From bb94c57ceec13cd8450f842ccbf70d1cca76b8c6 Mon Sep 17 00:00:00 2001 From: robertl Date: Tue, 17 Sep 2002 21:58:36 +0000 Subject: [PATCH] Add Garmin serial support, largely via jeeps 0.1.3. --- gpsbabel/Makefile | 9 +- gpsbabel/jeeps/README | 1 + gpsbabel/jeeps/gps.h | 196 ++ gpsbabel/jeeps/gpsapp.c | 5386 +++++++++++++++++++++++++++++++++++ gpsbabel/jeeps/gpsapp.h | 62 + gpsbabel/jeeps/gpscom.c | 606 ++++ gpsbabel/jeeps/gpscom.h | 45 + gpsbabel/jeeps/gpsdatum.h | 206 ++ gpsbabel/jeeps/gpsfmt.c | 1468 ++++++++++ gpsbabel/jeeps/gpsfmt.h | 27 + gpsbabel/jeeps/gpsinput.c | 2107 ++++++++++++++ gpsbabel/jeeps/gpsinput.h | 23 + gpsbabel/jeeps/gpsmath.c | 1798 ++++++++++++ gpsbabel/jeeps/gpsmath.h | 123 + gpsbabel/jeeps/gpsmem.c | 1459 ++++++++++ gpsbabel/jeeps/gpsmem.h | 88 + gpsbabel/jeeps/gpsnmea.h | 312 ++ gpsbabel/jeeps/gpsnmeafmt.h | 44 + gpsbabel/jeeps/gpsnmeaget.h | 43 + gpsbabel/jeeps/gpsport.h | 16 + gpsbabel/jeeps/gpsproj.c | 4485 +++++++++++++++++++++++++++++ gpsbabel/jeeps/gpsproj.h | 157 + gpsbabel/jeeps/gpsprot.c | 366 +++ gpsbabel/jeeps/gpsprot.h | 271 ++ gpsbabel/jeeps/gpsread.c | 236 ++ gpsbabel/jeeps/gpsread.h | 23 + gpsbabel/jeeps/gpsrqst.c | 175 ++ gpsbabel/jeeps/gpsrqst.h | 20 + gpsbabel/jeeps/gpssend.c | 178 ++ gpsbabel/jeeps/gpssend.h | 26 + gpsbabel/jeeps/gpsserial.c | 542 ++++ gpsbabel/jeeps/gpsserial.h | 31 + gpsbabel/jeeps/gpsutil.c | 681 +++++ gpsbabel/jeeps/gpsutil.h | 46 + gpsbabel/jeeps/main.c | 31 + gpsbabel/mingw/Makefile | 2 +- gpsbabel/mkshort.c | 46 +- gpsbabel/psp.c | 4 + gpsbabel/vecs.c | 6 + 39 files changed, 21324 insertions(+), 21 deletions(-) create mode 100644 gpsbabel/jeeps/README create mode 100644 gpsbabel/jeeps/gps.h create mode 100644 gpsbabel/jeeps/gpsapp.c create mode 100644 gpsbabel/jeeps/gpsapp.h create mode 100644 gpsbabel/jeeps/gpscom.c create mode 100644 gpsbabel/jeeps/gpscom.h create mode 100644 gpsbabel/jeeps/gpsdatum.h create mode 100644 gpsbabel/jeeps/gpsfmt.c create mode 100644 gpsbabel/jeeps/gpsfmt.h create mode 100644 gpsbabel/jeeps/gpsinput.c create mode 100644 gpsbabel/jeeps/gpsinput.h create mode 100644 gpsbabel/jeeps/gpsmath.c create mode 100644 gpsbabel/jeeps/gpsmath.h create mode 100644 gpsbabel/jeeps/gpsmem.c create mode 100644 gpsbabel/jeeps/gpsmem.h create mode 100644 gpsbabel/jeeps/gpsnmea.h create mode 100644 gpsbabel/jeeps/gpsnmeafmt.h create mode 100644 gpsbabel/jeeps/gpsnmeaget.h create mode 100644 gpsbabel/jeeps/gpsport.h create mode 100644 gpsbabel/jeeps/gpsproj.c create mode 100644 gpsbabel/jeeps/gpsproj.h create mode 100644 gpsbabel/jeeps/gpsprot.c create mode 100644 gpsbabel/jeeps/gpsprot.h create mode 100644 gpsbabel/jeeps/gpsread.c create mode 100644 gpsbabel/jeeps/gpsread.h create mode 100644 gpsbabel/jeeps/gpsrqst.c create mode 100644 gpsbabel/jeeps/gpsrqst.h create mode 100644 gpsbabel/jeeps/gpssend.c create mode 100644 gpsbabel/jeeps/gpssend.h create mode 100644 gpsbabel/jeeps/gpsserial.c create mode 100644 gpsbabel/jeeps/gpsserial.h create mode 100644 gpsbabel/jeeps/gpsutil.c create mode 100644 gpsbabel/jeeps/gpsutil.h create mode 100644 gpsbabel/jeeps/main.c diff --git a/gpsbabel/Makefile b/gpsbabel/Makefile index 1aac929d1..6f324bfd2 100644 --- a/gpsbabel/Makefile +++ b/gpsbabel/Makefile @@ -2,10 +2,15 @@ CFLAGS=-g -Icoldsync FMTS=magproto.o gpx.o geo.o gpsman.o mapsend.o mapsource.o \ gpsutil.o tiger.o pcx.o csv.o cetus.o gpspilot.o magnav.o \ - psp.o mxf.o holux.o + psp.o mxf.o holux.o garmin.o + +JEEPS=jeeps/gpsapp.o jeeps/gpscom.o jeeps/gpsfmt.o jeeps/gpsinput.o \ + jeeps/gpsmath.o jeeps/gpsmem.o \ + jeeps/gpsproj.o jeeps/gpsprot.o jeeps/gpsread.o \ + jeeps/gpsrqst.o jeeps/gpssend.o jeeps/gpsserial.o jeeps/gpsutil.o OBJS=main.o queue.o route.o waypt.o util.o vecs.o mkshort.o \ - coldsync/util.o coldsync/pdb.o $(FMTS) + coldsync/util.o coldsync/pdb.o $(GARMIN) $(JEEPS) $(FMTS) all: gpsbabel diff --git a/gpsbabel/jeeps/README b/gpsbabel/jeeps/README new file mode 100644 index 000000000..f626fc4c5 --- /dev/null +++ b/gpsbabel/jeeps/README @@ -0,0 +1 @@ +This is from jeeps-0.1.3. This code is under GPL. diff --git a/gpsbabel/jeeps/gps.h b/gpsbabel/jeeps/gps.h new file mode 100644 index 000000000..e2d6f4d27 --- /dev/null +++ b/gpsbabel/jeeps/gps.h @@ -0,0 +1,196 @@ +#ifdef __cplusplus +extern "C" +{ +#endif + +#ifndef gps_h +#define gps_h + +#include "gpsport.h" +#include + +#define FRAMING_ERROR -1 +#define PROTOCOL_ERROR -2 +#define HARDWARE_ERROR -3 +#define SERIAL_ERROR -4 +#define MEMORY_ERROR -5 +#define GPS_UNSUPPORTED -6 +#define INPUT_ERROR -7 + +#define MAX_GPS_PACKET_SIZE 1024 +#define GPS_TIME_OUT 5 + +#define gpsTrue 1 +#define gpsFalse 0 + +#define DLE 0x10 +#define ETX 0x03 + + +extern int32 gps_errno; +extern int32 gps_warning; +extern int32 gps_error; +extern int32 gps_user; +extern int32 gps_show_bytes; + + +typedef struct GPS_SPacket +{ + UC dle; + UC type; + UC n; + UC *data; + UC chk; + UC edle; + UC etx; + UC bytes; /* Actual number of bytes (for sending) */ +} GPS_OPacket, *GPS_PPacket; + + + +typedef struct GPS_SProduct_Data_Type +{ + int16 id; + int16 version; + char desc[MAX_GPS_PACKET_SIZE]; +} GPS_OProduct_Data_Type, *GPS_PProduct_Data_Type; + + + + +typedef struct GPS_SPvt_Data_Type +{ + float alt; + float epe; + float eph; + float epv; + int16 fix; + double tow; + double lat; + double lon; + float east; + float north; + float up; + float msl_hght; + int16 leap_scnds; + int32 wn_days; +} GPS_OPvt_Data, *GPS_PPvt_Data; + + + +typedef struct GPS_STrack +{ + double lat; /* Degrees */ + double lon; /* Degrees */ + time_t Time; /* Unix time */ + float alt; /* Altitude */ + float dpth; /* Depth */ + int32 tnew; /* New track? */ + int32 ishdr; /* Track header? */ + int32 dspl; /* Display on map? */ + int32 colour; /* Colour */ + char trk_ident[256]; /* Track identifier */ +} +GPS_OTrack, *GPS_PTrack; + + + +typedef struct GPS_SAlmanac +{ + UC svid; + int16 wn; + float toa; + float af0; + float af1; + float e; + float sqrta; + float m0; + float w; + float omg0; + float odot; + float i; + UC hlth; +} GPS_OAlmanac, *GPS_PAlmanac; + + +typedef struct GPS_SWay +{ + char ident[256]; + double lat; + double lon; + char cmnt[256]; + float dst; + int32 smbl; + int32 dspl; + char wpt_ident[256]; + char lnk_ident[256]; + UC subclass[18]; + int32 colour; + char cc[2]; + UC wpt_class; + int32 alt; + char city[24]; + char state[2]; + char name[30]; + char facility[32]; + char addr[52]; + char cross_road[52]; + int32 attr; + float dpth; + int32 idx; + int32 prot; + int32 isrte; + int32 rte_prot; + UC rte_num; + char rte_cmnt[20]; + char rte_ident[256]; + int32 islink; + int32 rte_link_class; + char rte_link_subclass[18]; + char rte_link_ident[256]; +} GPS_OWay, *GPS_PWay; + + + + +#include "gpsserial.h" +#include "gpssend.h" +#include "gpsread.h" +#include "gpsutil.h" +#include "gpsapp.h" +#include "gpsprot.h" +#include "gpscom.h" +#include "gpsfmt.h" +#include "gpsmath.h" +#include "gpsnmea.h" +#include "gpsmem.h" +#include "gpsrqst.h" +#include "gpsinput.h" +#include "gpsproj.h" +#include "gpsnmeafmt.h" +#include "gpsnmeaget.h" + +time_t gps_save_time; +double gps_save_lat; +double gps_save_lon; +extern int32 gps_save_id; +extern double gps_save_version; +extern char gps_save_string[GPS_ARB_LEN]; + + +extern struct COMMANDDATA COMMAND_ID[2]; +extern struct LINKDATA LINK_ID[3]; +extern struct GPS_MODEL_PROTOCOL GPS_MP[]; + +extern char *gps_marine_sym[]; +extern char *gps_land_sym[]; +extern char *gps_aviation_sym[]; +extern char *gps_16_sym[]; + + + +#endif + +#ifdef __cplusplus +} +#endif diff --git a/gpsbabel/jeeps/gpsapp.c b/gpsbabel/jeeps/gpsapp.c new file mode 100644 index 000000000..6987726b9 --- /dev/null +++ b/gpsbabel/jeeps/gpsapp.c @@ -0,0 +1,5386 @@ +/******************************************************************** +** @source JEEPS application and data functions +** +** @author Copyright (C) 1999 Alan Bleasby +** @version 1.0 +** @modified Dec 28 1999 Alan Bleasby. First version +** @@ +** +** This library is free software; you can redistribute it and/or +** modify it under the terms of the GNU Library General Public +** License as published by the Free Software Foundation; either +** version 2 of the License, or (at your option) any later version. +** +** This library is distributed in the hope that it will be useful, +** but WITHOUT ANY WARRANTY; without even the implied warranty of +** MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU +** Library General Public License for more details. +** +** You should have received a copy of the GNU Library General Public +** License along with this library; if not, write to the +** Free Software Foundation, Inc., 59 Temple Place - Suite 330, +** Boston, MA 02111-1307, USA. +********************************************************************/ +#include "gps.h" +#include +#include +#include +#include + + +static int32 GPS_A000(const char *port); +static void GPS_A001(GPS_PPacket packet); + + +static void GPS_A500_Translate(UC *s, GPS_PAlmanac *alm); +static void GPS_A500_Encode(UC *s, GPS_PAlmanac alm); +static void GPS_A300_Translate(UC *s, GPS_PTrack *trk); +static void GPS_A300_Encode(UC *s, GPS_PTrack trk); + + +static void GPS_D100_Get(GPS_PWay *way, UC *s); +static void GPS_D101_Get(GPS_PWay *way, UC *s); +static void GPS_D102_Get(GPS_PWay *way, UC *s); +static void GPS_D103_Get(GPS_PWay *way, UC *s); +static void GPS_D104_Get(GPS_PWay *way, UC *s); +static void GPS_D105_Get(GPS_PWay *way, UC *s); +static void GPS_D106_Get(GPS_PWay *way, UC *s); +static void GPS_D107_Get(GPS_PWay *way, UC *s); +static void GPS_D108_Get(GPS_PWay *way, UC *s); +static void GPS_D150_Get(GPS_PWay *way, UC *s); +static void GPS_D151_Get(GPS_PWay *way, UC *s); +static void GPS_D152_Get(GPS_PWay *way, UC *s); +static void GPS_D154_Get(GPS_PWay *way, UC *s); +static void GPS_D155_Get(GPS_PWay *way, UC *s); + +static void GPS_D100_Send(UC *data, GPS_PWay way, int32 *len); +static void GPS_D101_Send(UC *data, GPS_PWay way, int32 *len); +static void GPS_D102_Send(UC *data, GPS_PWay way, int32 *len); +static void GPS_D103_Send(UC *data, GPS_PWay way, int32 *len); +static void GPS_D104_Send(UC *data, GPS_PWay way, int32 *len); +static void GPS_D105_Send(UC *data, GPS_PWay way, int32 *len); +static void GPS_D106_Send(UC *data, GPS_PWay way, int32 *len); +static void GPS_D107_Send(UC *data, GPS_PWay way, int32 *len); +static void GPS_D108_Send(UC *data, GPS_PWay way, int32 *len); +static void GPS_D150_Send(UC *data, GPS_PWay way, int32 *len); +static void GPS_D151_Send(UC *data, GPS_PWay way, int32 *len); +static void GPS_D152_Send(UC *data, GPS_PWay way, int32 *len); +static void GPS_D154_Send(UC *data, GPS_PWay way, int32 *len); +static void GPS_D155_Send(UC *data, GPS_PWay way, int32 *len); + +static void GPS_D200_Get(GPS_PWay *way, UC *s); +static void GPS_D201_Get(GPS_PWay *way, UC *s); +static void GPS_D202_Get(GPS_PWay *way, UC *s); +static void GPS_D210_Get(GPS_PWay *way, UC *s); +static void GPS_D200_Send(UC *data, GPS_PWay way, int32 *len); +static void GPS_D201_Send(UC *data, GPS_PWay way, int32 *len); +static void GPS_D202_Send(UC *data, GPS_PWay way, int32 *len); +static void GPS_D210_Send(UC *data, GPS_PWay way, int32 *len); + +static void GPS_D400_Get(GPS_PWay *way, UC *s); +static void GPS_D403_Get(GPS_PWay *way, UC *s); +static void GPS_D450_Get(GPS_PWay *way, UC *s); +static void GPS_D400_Send(UC *data, GPS_PWay way, int32 *len); +static void GPS_D403_Send(UC *data, GPS_PWay way, int32 *len); +static void GPS_D450_Send(UC *data, GPS_PWay way, int32 *len); + +static int32 GPS_D500_Get(GPS_PAlmanac *alm, int32 entries, int32 fd); +static int32 GPS_D501_Get(GPS_PAlmanac *alm, int32 entries, int32 fd); +static int32 GPS_D550_Get(GPS_PAlmanac *alm, int32 entries, int32 fd); +static int32 GPS_D551_Get(GPS_PAlmanac *alm, int32 entries, int32 fd); +static void GPS_D500_Send(UC *data, GPS_PAlmanac alm); +static void GPS_D501_Send(UC *data, GPS_PAlmanac alm); +static void GPS_D550_Send(UC *data, GPS_PAlmanac alm); +static void GPS_D551_Send(UC *data, GPS_PAlmanac alm); + + +int32 gps_save_id; +double gps_save_version; +char gps_save_string[GPS_ARB_LEN]; + + +/* @func GPS_Init ****************************************************** +** +** Initialise GPS communication +** Get capabilities and store time lat/lon in case GPS requests +** it later. +** Find endian nature of hardware and store +** +** @param [r] port [const char *] serial port +** +** @return [int32] 1 if success -ve if error +************************************************************************/ +int32 GPS_Init(const char *port) +{ + int32 ret; + + (void) GPS_Util_Little(); + ret = GPS_A000(port); + if(ret<0) return ret; + + gps_save_time = GPS_Command_Get_Time(port); + if(!gps_save_time) { + return FRAMING_ERROR; + } + return GPS_Command_Get_Position(port,&gps_save_lat,&gps_save_lon); +} + + +/* @funcstatic GPS_A000 ************************************************ +** +** Return product ID, version and description. Turn off PVT transfer +** +** @param [r] port [const char *] serial port +** +** @return [int32] 1 if success -ve if error +************************************************************************/ +static int32 GPS_A000(const char *port) +{ + int32 fd; + GPS_PPacket tra; + GPS_PPacket rec; + int16 version; + int16 id; + char tstr[256]; + + if(!GPS_Serial_On(port, &fd)) + return gps_errno; + + if(!GPS_Serial_Flush(fd)) + return gps_errno; + + if(!(tra = GPS_Packet_New()) || !(rec = GPS_Packet_New())) + return MEMORY_ERROR; + + GPS_Make_Packet(&tra, LINK_ID[0].Pid_Product_Rqst,NULL,0); + if(!GPS_Write_Packet(fd,tra)) + return SERIAL_ERROR; + + if(!GPS_Get_Ack(fd, &tra, &rec)) + return SERIAL_ERROR; + + GPS_Packet_Read(fd, &rec); + GPS_Send_Ack(fd, &tra, &rec); + + id = GPS_Util_Get_Short(rec->data); + version = GPS_Util_Get_Short((rec->data)+2); + + (void) strcpy(gps_save_string,(char *)rec->data+4); + GPS_User((char *)rec->data+4); + (void) sprintf(tstr,"ID:\t\t%d\n",id); + gps_save_id = id; + GPS_User(tstr); + gps_save_version = (double)((double)version/(double)100.); + (void) sprintf(tstr, + "Version:\t%.2f\n",gps_save_version); + GPS_User(tstr); + + + + + gps_date_time_transfer = pA600; + gps_date_time_type = pD600; /* All models so far */ + gps_position_transfer = pA700; + gps_position_type = pD700; /* All models so far */ + gps_pvt_transfer = -1; + gps_pvt_type = -1; + gps_prx_waypt_transfer = -1; + gps_prx_waypt_type = -1; + gps_trk_transfer = -1; + gps_trk_type = -1; + gps_trk_hdr_type = -1; + gps_rte_link_type = -1; + + if(!GPS_Serial_Wait(fd)) + { + GPS_Warning("A001 protocol not supported"); + id = GPS_Protocol_Version_Change(id,version); + if(GPS_Protocol_Table_Set(id)<0) + return GPS_UNSUPPORTED; + } + else + { + (void) GPS_Packet_Read(fd, &rec); + GPS_Send_Ack(fd, &tra, &rec); + GPS_A001(rec); + } + + + /* Make sure PVT is off as some GPS' have it on by default */ + if(gps_pvt_transfer != -1) + GPS_A800_Off(port,&fd); + + + GPS_Packet_Del(&tra); + GPS_Packet_Del(&rec); + + if(!GPS_Serial_Off(port, fd)) + return gps_errno; + + return 1; +} + + + + +/* @funcstatic GPS_A001 ************************************************ +** +** Extract protocol capabilities +** This routine could do with re-writing. It's too long and obtuse. +** +** @param [r] packet [GPS_PPacket] A001 protocol packet +** +** @return [void] +************************************************************************/ +static void GPS_A001(GPS_PPacket packet) +{ + int32 entries; + int32 i; + UC *p; + US tag; + US data; + US lasta=0; + + gps_link_type = -1; + gps_device_command = -1; + gps_waypt_transfer = -1; + gps_waypt_type = -1; + gps_route_transfer = -1; + gps_rte_hdr_type = -1; + gps_rte_type = -1; + gps_trk_transfer = -1; + gps_trk_type = -1; + gps_prx_waypt_transfer = -1; + gps_prx_waypt_type = -1; + gps_almanac_transfer = -1; + gps_almanac_type = -1; + + entries = packet->n / 3; + p = packet->data; + + for(i=0;i=100) + { + gps_waypt_type = data; + continue; + } + if(data<153 && data>=150) + { + gps_waypt_type = data; + continue; + } + if(data<156 && data>=154) + { + gps_waypt_type = data; + continue; + } + else + GPS_Protocol_Error(tag,data); + } + + + else if(lasta<300) + { + if(data>=200 && data <=202) + { + gps_rte_hdr_type = data; + continue; + } + if(data==210) + { + gps_rte_link_type = data; + continue; + } + + if(data<109 && data>=100) + { + gps_rte_type = data; + continue; + } + if(data<153 && data>=150) + { + gps_rte_type = data; + continue; + } + if(data<156 && data>=154) + { + gps_rte_type = data; + continue; + } + if(data<451) + { + if(data==400) + gps_rte_type = pD400; + else if(data==403) + gps_rte_type = pD403; + else if(data==450) + gps_rte_type = pD450; + else + GPS_Protocol_Error(tag,data); + continue; + } + } + + else if(lasta<400) + { + if(data==300) + gps_trk_type = pD300; + else if(data==301) + gps_trk_type = pD301; + else if(data==310) + gps_trk_hdr_type = pD310; + else + GPS_Protocol_Error(tag,data); + continue; + } + + + else if(lasta<500) + { + if(data<109 && data>=100) + { + gps_prx_waypt_type = data; + continue; + } + if(data<153 && data>=150) + { + gps_prx_waypt_type = data; + continue; + } + if(data<156 && data>=154) + { + gps_prx_waypt_type = data; + continue; + } + if(data<451) + { + if(data==400) + gps_prx_waypt_type = pD400; + else if(data==403) + gps_prx_waypt_type = pD403; + else if(data==450) + gps_prx_waypt_type = pD450; + else + GPS_Protocol_Error(tag,data); + continue; + } + } + + else if(lasta<600) + { + if(data==500) + gps_almanac_type = pD500; + else if(data==501) + gps_almanac_type = pD501; + else if(data==550) + gps_almanac_type = pD550; + else if(data==551) + gps_almanac_type = pD551; + else + GPS_Protocol_Error(tag,data); + continue; + } + + else if(lasta<700) + { + if(data!=600) + GPS_Protocol_Error(tag,data); + else + gps_date_time_type = pD600; + continue; + } + else if(lasta<800) + { + if(data!=700) + GPS_Protocol_Error(tag,data); + else + gps_position_type = pD700; + continue; + } + else if(lasta<900) + { + if(data!=800) + GPS_Protocol_Error(tag,data); + else + gps_pvt_type = pD800; + continue; + } + + + } + } + + return; +} + + + + +/* @func GPS_A100_Get ****************************************************** +** +** Get waypoint data from GPS +** +** @param [r] port [const char *] serial port +** @param [w] way [GPS_PWay **] waypoint array +** +** @return [int32] number of waypoint entries +************************************************************************/ +int32 GPS_A100_Get(const char *port, GPS_PWay **way) +{ + static UC data[2]; + int32 fd; + GPS_PPacket tra; + GPS_PPacket rec; + int32 n; + int32 i; + + + if(!GPS_Serial_On(port,&fd)) + return gps_errno; + + if(!(tra = GPS_Packet_New()) || !(rec = GPS_Packet_New())) + return MEMORY_ERROR; + + GPS_Util_Put_Short(data, + COMMAND_ID[gps_device_command].Cmnd_Transfer_Wpt); + GPS_Make_Packet(&tra, LINK_ID[gps_link_type].Pid_Command_Data, + data,2); + + if(!GPS_Write_Packet(fd,tra)) + { + GPS_Error("A100_Get: Cannot write packet"); + return FRAMING_ERROR; + } + + if(!GPS_Get_Ack(fd, &tra, &rec)) + { + GPS_Error("A100_Get: No acknowledge"); + return FRAMING_ERROR; + } + + GPS_Packet_Read(fd, &rec); + GPS_Send_Ack(fd, &tra, &rec); + + n = GPS_Util_Get_Short(rec->data); + + if(n) + if(!((*way)=(GPS_PWay *)malloc(n*sizeof(GPS_PWay)))) + { + GPS_Error("A100_Get: Insufficient memory"); + return MEMORY_ERROR; + } + + for(i=0;idata); + break; + case pD101: + GPS_D101_Get(&((*way)[i]),rec->data); + break; + case pD102: + GPS_D102_Get(&((*way)[i]),rec->data); + break; + case pD103: + GPS_D103_Get(&((*way)[i]),rec->data); + break; + case pD104: + GPS_D104_Get(&((*way)[i]),rec->data); + break; + case pD105: + GPS_D105_Get(&((*way)[i]),rec->data); + break; + case pD106: + GPS_D106_Get(&((*way)[i]),rec->data); + break; + case pD107: + GPS_D107_Get(&((*way)[i]),rec->data); + break; + case pD108: + GPS_D108_Get(&((*way)[i]),rec->data); + break; + case pD150: + GPS_D150_Get(&((*way)[i]),rec->data); + break; + case pD151: + GPS_D151_Get(&((*way)[i]),rec->data); + break; + case pD152: + GPS_D152_Get(&((*way)[i]),rec->data); + break; + case pD154: + GPS_D154_Get(&((*way)[i]),rec->data); + break; + case pD155: + GPS_D155_Get(&((*way)[i]),rec->data); + break; + default: + GPS_Error("A100_GET: Unknown waypoint protocol"); + return PROTOCOL_ERROR; + } + } + + if(!GPS_Packet_Read(fd, &rec)) + return gps_errno; + if(!GPS_Send_Ack(fd, &tra, &rec)) + return gps_errno; + + if(rec->type != LINK_ID[gps_link_type].Pid_Xfer_Cmplt) + { + GPS_Error("A100_GET: Error transferring waypoints"); + return FRAMING_ERROR; + } + + if(i != n) + { + GPS_Error("A100_GET: Waypoint entry number mismatch"); + return FRAMING_ERROR; + } + + GPS_Packet_Del(&tra); + GPS_Packet_Del(&rec); + + if(!GPS_Serial_Off(port, fd)) + return gps_errno; + + return n; +} + + + + + +/* @func GPS_A100_Send ************************************************** +** +** Send waypoints to GPS +** +** @param [r] port [const char *] serial port +** @param [r] trk [GPS_PWay *] waypoint array +** @param [r] n [int32] number of waypoint entries +** +** @return [int32] success +************************************************************************/ +int32 GPS_A100_Send(const char *port, GPS_PWay *way, int32 n) +{ + UC data[GPS_ARB_LEN]; + int32 fd; + GPS_PPacket tra; + GPS_PPacket rec; + int32 i; + int32 len; + + if(!GPS_Serial_On(port,&fd)) + return gps_errno; + + if(!(tra = GPS_Packet_New()) || !(rec = GPS_Packet_New())) + return MEMORY_ERROR; + + GPS_Util_Put_Short(data,n); + GPS_Make_Packet(&tra, LINK_ID[gps_link_type].Pid_Records, + data,2); + if(!GPS_Write_Packet(fd,tra)) + return gps_errno; + if(!GPS_Get_Ack(fd, &tra, &rec)) + { + GPS_Error("Waypoint start data not acknowledged"); + return gps_errno; + } + + + for(i=0;iprot = 100; + for(i=0;i<6;++i) (*way)->ident[i] = *p++; + + (*way)->lat = GPS_Math_Semi_To_Deg(GPS_Util_Get_Int(p)); + p+=sizeof(int32); + + (*way)->lon = GPS_Math_Semi_To_Deg(GPS_Util_Get_Int(p)); + p+=sizeof(int32); + + p+=sizeof(int32); + + for(i=0;i<40;++i) (*way)->cmnt[i] = *p++; + + return; +} + + + +/* @funcstatic GPS_D101_Get ********************************************* +** +** Get waypoint data +** +** @param [w] way [GPS_PWay *] waypoint array +** @param [r] s [UC *] packet data +** +** @return [void] +************************************************************************/ +static void GPS_D101_Get(GPS_PWay *way, UC *s) +{ + UC *p; + int32 i; + + p=s; + + (*way)->prot = 101; + for(i=0;i<6;++i) (*way)->ident[i] = *p++; + + (*way)->lat = GPS_Math_Semi_To_Deg(GPS_Util_Get_Int(p)); + p+=sizeof(int32); + + (*way)->lon = GPS_Math_Semi_To_Deg(GPS_Util_Get_Int(p)); + p+=sizeof(int32); + + p+=sizeof(int32); + + for(i=0;i<40;++i) (*way)->cmnt[i] = *p++; + + (*way)->dst = GPS_Util_Get_Float(p); + p+=sizeof(float); + + (*way)->smbl = *p; + + return; +} + + + +/* @funcstatic GPS_D102_Get ******************************************** +** +** Get waypoint data +** +** @param [w] way [GPS_PWay *] waypoint array +** @param [r] s [UC *] packet data +** +** @return [void] +************************************************************************/ +static void GPS_D102_Get(GPS_PWay *way, UC *s) +{ + UC *p; + int32 i; + + p=s; + + (*way)->prot = 102; + for(i=0;i<6;++i) (*way)->ident[i] = *p++; + + (*way)->lat = GPS_Math_Semi_To_Deg(GPS_Util_Get_Int(p)); + p+=sizeof(int32); + + (*way)->lon = GPS_Math_Semi_To_Deg(GPS_Util_Get_Int(p)); + p+=sizeof(int32); + + p+=sizeof(int32); + + for(i=0;i<40;++i) (*way)->cmnt[i] = *p++; + + (*way)->dst = GPS_Util_Get_Float(p); + p+=sizeof(float); + + (*way)->smbl = GPS_Util_Get_Short(p); + + + return; +} + + + +/* @funcstatic GPS_D103_Get ********************************************* +** +** Get waypoint data +** +** @param [w] way [GPS_PWay *] waypoint array +** @param [r] s [UC *] packet data +** +** @return [void] +************************************************************************/ +static void GPS_D103_Get(GPS_PWay *way, UC *s) +{ + UC *p; + int32 i; + + p=s; + + (*way)->prot = 103; + for(i=0;i<6;++i) (*way)->ident[i] = *p++; + + (*way)->lat = GPS_Math_Semi_To_Deg(GPS_Util_Get_Int(p)); + p+=sizeof(int32); + + (*way)->lon = GPS_Math_Semi_To_Deg(GPS_Util_Get_Int(p)); + p+=sizeof(int32); + + p+=sizeof(int32); + + for(i=0;i<40;++i) (*way)->cmnt[i] = *p++; + + (*way)->smbl = *p++; + (*way)->dspl = *p; + + + return; +} + + + +/* @funcstatic GPS_D104_Get ******************************************** +** +** Get waypoint data +** +** @param [w] way [GPS_PWay *] waypoint array +** @param [r] s [UC *] packet data +** +** @return [void] +************************************************************************/ +static void GPS_D104_Get(GPS_PWay *way, UC *s) +{ + UC *p; + int32 i; + + p=s; + + (*way)->prot = 104; + for(i=0;i<6;++i) (*way)->ident[i] = *p++; + + (*way)->lat = GPS_Math_Semi_To_Deg(GPS_Util_Get_Int(p)); + p+=sizeof(int32); + + (*way)->lon = GPS_Math_Semi_To_Deg(GPS_Util_Get_Int(p)); + p+=sizeof(int32); + + p+=sizeof(int32); + + for(i=0;i<40;++i) (*way)->cmnt[i] = *p++; + + (*way)->dst = GPS_Util_Get_Float(p); + p+=sizeof(float); + + (*way)->smbl = GPS_Util_Get_Short(p); + p+=sizeof(int16); + + (*way)->dspl = *p; + + return; +} + + + +/* @funcstatic GPS_D105_Get ******************************************** +** +** Get waypoint data +** +** @param [w] way [GPS_PWay *] waypoint array +** @param [r] s [UC *] packet data +** +** @return [void] +************************************************************************/ +static void GPS_D105_Get(GPS_PWay *way, UC *s) +{ + UC *p; + UC *q; + + p=s; + + (*way)->prot = 105; + + (*way)->lat = GPS_Math_Semi_To_Deg(GPS_Util_Get_Int(p)); + p+=sizeof(int32); + + (*way)->lon = GPS_Math_Semi_To_Deg(GPS_Util_Get_Int(p)); + p+=sizeof(int32); + + (*way)->smbl = GPS_Util_Get_Short(p); + p+=sizeof(int16); + + q = (UC *) (*way)->wpt_ident; + while((*q++ = *p++)); + + return; +} + + + +/* @funcstatic GPS_D106_Get ******************************************** +** +** Get waypoint data +** +** @param [w] way [GPS_PWay *] waypoint array +** @param [r] s [UC *] packet data +** +** @return [void] +************************************************************************/ +void GPS_D106_Get(GPS_PWay *way, UC *s) +{ + UC *p; + UC *q; + int32 i; + + p=s; + + (*way)->prot = 106; + + (*way)->wpt_class = *p++; + + for(i=0;i<13;++i) (*way)->subclass[i] = *p++; + + (*way)->lat = GPS_Math_Semi_To_Deg(GPS_Util_Get_Int(p)); + p+=sizeof(int32); + + (*way)->lon = GPS_Math_Semi_To_Deg(GPS_Util_Get_Int(p)); + p+=sizeof(int32); + + (*way)->smbl = GPS_Util_Get_Short(p); + p+=sizeof(int16); + + q = (UC *) (*way)->wpt_ident; + while((*q++ = *p++)); + q = (UC *) (*way)->lnk_ident; + while((*q++ = *p++)); + + return; +} + + + +/* @funcstatic GPS_D107_Get ******************************************** +** +** Get waypoint data +** +** @param [w] way [GPS_PWay *] waypoint array +** @param [r] s [UC *] packet data +** +** @return [void] +************************************************************************/ +static void GPS_D107_Get(GPS_PWay *way, UC *s) +{ + UC *p; + int32 i; + + p=s; + + (*way)->prot = 107; + for(i=0;i<6;++i) (*way)->ident[i] = *p++; + + (*way)->lat = GPS_Math_Semi_To_Deg(GPS_Util_Get_Int(p)); + p+=sizeof(int32); + + (*way)->lon = GPS_Math_Semi_To_Deg(GPS_Util_Get_Int(p)); + p+=sizeof(int32); + + p+=sizeof(int32); + + for(i=0;i<40;++i) (*way)->cmnt[i] = *p++; + + (*way)->smbl = *p++; + (*way)->dspl = *p++; + + (*way)->dst = GPS_Util_Get_Float(p); + p+=sizeof(float); + + (*way)->colour = *p++; + + return; +} + + + +/* @funcstatic GPS_D108_Get ******************************************** +** +** Get waypoint data +** +** @param [w] way [GPS_PWay *] waypoint array +** @param [r] s [UC *] packet data +** +** @return [void] +************************************************************************/ +static void GPS_D108_Get(GPS_PWay *way, UC *s) +{ + UC *p; + UC *q; + + int32 i; + + p=s; + + (*way)->prot = 108; + + (*way)->wpt_class = *p++; + (*way)->colour = *p++; + (*way)->dspl = *p++; + (*way)->attr = *p++; + (*way)->smbl = GPS_Util_Get_Short(p); + p+=sizeof(int16); + for(i=0;i<18;++i) (*way)->subclass[i] = *p++; + + (*way)->lat = GPS_Math_Semi_To_Deg(GPS_Util_Get_Int(p)); + p+=sizeof(int32); + + (*way)->lon = GPS_Math_Semi_To_Deg(GPS_Util_Get_Int(p)); + p+=sizeof(int32); + + (*way)->alt = (int32)GPS_Util_Get_Float(p); + p+=sizeof(float); + (*way)->dpth = (int32)GPS_Util_Get_Float(p); + p+=sizeof(float); + (*way)->dst = (int32)GPS_Util_Get_Float(p); + p+=sizeof(float); + + for(i=0;i<2;++i) (*way)->state[i] = *p++; + for(i=0;i<2;++i) (*way)->cc[i] = *p++; + + q = (UC *) (*way)->ident; + while((*q++ = *p++)); + + q = (UC *) (*way)->cmnt; + while((*q++ = *p++)); + + q = (UC *) (*way)->facility; + while((*q++ = *p++)); + + q = (UC *) (*way)->city; + while((*q++ = *p++)); + + q = (UC *) (*way)->addr; + while((*q++ = *p++)); + + q = (UC *) (*way)->cross_road; + while((*q++ = *p++)); + + return; +} + + + +/* @funcstatic GPS_D150_Get ******************************************** +** +** Get waypoint data +** +** @param [w] way [GPS_PWay *] waypoint array +** @param [r] s [UC *] packet data +** +** @return [void] +************************************************************************/ +static void GPS_D150_Get(GPS_PWay *way, UC *s) +{ + UC *p; + int32 i; + + p=s; + + (*way)->prot = 150; + for(i=0;i<6;++i) (*way)->ident[i] = *p++; + for(i=0;i<2;++i) (*way)->cc[i] = *p++; + (*way)->wpt_class = *p++; + + (*way)->lat = GPS_Math_Semi_To_Deg(GPS_Util_Get_Int(p)); + p+=sizeof(int32); + + (*way)->lon = GPS_Math_Semi_To_Deg(GPS_Util_Get_Int(p)); + p+=sizeof(int32); + + (*way)->alt = GPS_Util_Get_Short(p); + p+=sizeof(int16); + + for(i=0;i<24;++i) (*way)->city[i] = *p++; + for(i=0;i<2;++i) (*way)->state[i] = *p++; + for(i=0;i<30;++i) (*way)->name[i] = *p++; + for(i=0;i<40;++i) (*way)->cmnt[i] = *p++; + + return; +} + + + +/* @funcstatic GPS_D151_Get ********************************************* +** +** Get waypoint data +** +** @param [w] way [GPS_PWay *] waypoint array +** @param [r] s [UC *] packet data +** +** @return [void] +************************************************************************/ +static void GPS_D151_Get(GPS_PWay *way, UC *s) +{ + UC *p; + int32 i; + + p=s; + + (*way)->prot = 151; + for(i=0;i<6;++i) (*way)->ident[i] = *p++; + + (*way)->lat = GPS_Math_Semi_To_Deg(GPS_Util_Get_Int(p)); + p+=sizeof(int32); + + (*way)->lon = GPS_Math_Semi_To_Deg(GPS_Util_Get_Int(p)); + p+=sizeof(int32); + + p+=sizeof(int32); + + for(i=0;i<40;++i) (*way)->cmnt[i] = *p++; + + (*way)->dst = GPS_Util_Get_Float(p); + p+=sizeof(float); + + for(i=0;i<30;++i) (*way)->name[i] = *p++; + for(i=0;i<24;++i) (*way)->city[i] = *p++; + for(i=0;i<2;++i) (*way)->state[i] = *p++; + + (*way)->alt = GPS_Util_Get_Short(p); + p+=sizeof(int16); + + for(i=0;i<2;++i) (*way)->cc[i] = *p++; + + ++p; + + (*way)->wpt_class = *p; + + return; +} + + + +/* @funcstatic GPS_D152_Get ******************************************** +** +** Get waypoint data +** +** @param [w] way [GPS_PWay *] waypoint array +** @param [r] s [UC *] packet data +** +** @return [void] +************************************************************************/ +static void GPS_D152_Get(GPS_PWay *way, UC *s) +{ + UC *p; + int32 i; + + p=s; + + (*way)->prot = 152; + for(i=0;i<6;++i) (*way)->ident[i] = *p++; + + (*way)->lat = GPS_Math_Semi_To_Deg(GPS_Util_Get_Int(p)); + p+=sizeof(int32); + + (*way)->lon = GPS_Math_Semi_To_Deg(GPS_Util_Get_Int(p)); + p+=sizeof(int32); + + p+=sizeof(int32); + + for(i=0;i<40;++i) (*way)->cmnt[i] = *p++; + + (*way)->dst = GPS_Util_Get_Float(p); + p+=sizeof(float); + + for(i=0;i<30;++i) (*way)->name[i] = *p++; + for(i=0;i<24;++i) (*way)->city[i] = *p++; + for(i=0;i<2;++i) (*way)->state[i] = *p++; + + (*way)->alt = GPS_Util_Get_Short(p); + p+=sizeof(int16); + + for(i=0;i<2;++i) (*way)->cc[i] = *p++; + + ++p; + + (*way)->wpt_class = *p; + + return; +} + + +/* @funcstatic GPS_D154_Get ******************************************** +** +** Get waypoint data +** +** @param [w] way [GPS_PWay *] waypoint array +** @param [r] s [UC *] packet data +** +** @return [void] +************************************************************************/ +static void GPS_D154_Get(GPS_PWay *way, UC *s) +{ + UC *p; + int32 i; + + p=s; + + (*way)->prot = 154; + for(i=0;i<6;++i) (*way)->ident[i] = *p++; + + (*way)->lat = GPS_Math_Semi_To_Deg(GPS_Util_Get_Int(p)); + p+=sizeof(int32); + + (*way)->lon = GPS_Math_Semi_To_Deg(GPS_Util_Get_Int(p)); + p+=sizeof(int32); + + p+=sizeof(int32); + + for(i=0;i<40;++i) (*way)->cmnt[i] = *p++; + + (*way)->dst = GPS_Util_Get_Float(p); + p+=sizeof(float); + + for(i=0;i<30;++i) (*way)->name[i] = *p++; + for(i=0;i<24;++i) (*way)->city[i] = *p++; + for(i=0;i<2;++i) (*way)->state[i] = *p++; + + (*way)->alt = GPS_Util_Get_Short(p); + p+=sizeof(int16); + + for(i=0;i<2;++i) (*way)->cc[i] = *p++; + + ++p; + + (*way)->wpt_class = *p++; + + (*way)->smbl = GPS_Util_Get_Short(p); + + return; +} + + +/* @funcstatic GPS_D155_Get ********************************************* +** +** Get waypoint data +** +** @param [w] way [GPS_PWay *] waypoint array +** @param [r] s [UC *] packet data +** +** @return [void] +************************************************************************/ +static void GPS_D155_Get(GPS_PWay *way, UC *s) +{ + UC *p; + int32 i; + + p=s; + + (*way)->prot = 155; + for(i=0;i<6;++i) (*way)->ident[i] = *p++; + + (*way)->lat = GPS_Math_Semi_To_Deg(GPS_Util_Get_Int(p)); + p+=sizeof(int32); + + (*way)->lon = GPS_Math_Semi_To_Deg(GPS_Util_Get_Int(p)); + p+=sizeof(int32); + + p+=sizeof(int32); + + for(i=0;i<40;++i) (*way)->cmnt[i] = *p++; + + (*way)->dst = GPS_Util_Get_Float(p); + p+=sizeof(float); + + for(i=0;i<30;++i) (*way)->name[i] = *p++; + for(i=0;i<24;++i) (*way)->city[i] = *p++; + for(i=0;i<2;++i) (*way)->state[i] = *p++; + + (*way)->alt = GPS_Util_Get_Short(p); + p+=sizeof(int16); + + for(i=0;i<2;++i) (*way)->cc[i] = *p++; + + ++p; + + (*way)->wpt_class = *p++; + + (*way)->smbl = GPS_Util_Get_Short(p); + p+=sizeof(int16); + + (*way)->dspl = *p; + + return; +} + + + +/* @funcstatic GPS_D100_Send ******************************************* +** +** Form waypoint data string +** +** @param [w] data [UC *] string to write to +** @param [r] way [GPS_PWay] waypoint data +** @param [w] len [int32 *] packet length +** +** @return [void] +************************************************************************/ +static void GPS_D100_Send(UC *data, GPS_PWay way, int32 *len) +{ + UC *p; + int32 i; + + p = data; + + for(i=0;i<6;++i) *p++ = way->ident[i]; + GPS_Util_Put_Int(p,(int32)GPS_Math_Deg_To_Semi(way->lat)); + p+=sizeof(int32); + GPS_Util_Put_Int(p,(int32)GPS_Math_Deg_To_Semi(way->lon)); + p+=sizeof(int32); + GPS_Util_Put_Uint(p,0); + p+=sizeof(int32); + for(i=0;i<40;++i) *p++ = way->cmnt[i]; + + *len = 58; + + return; +} + + +/* @funcstatic GPS_D101_Send ******************************************** +** +** Form waypoint data string +** +** @param [w] data [UC *] string to write to +** @param [r] way [GPS_PWay] waypoint data +** @param [w] len [int32 *] packet length +** +** @return [void] +************************************************************************/ +static void GPS_D101_Send(UC *data, GPS_PWay way, int32 *len) +{ + UC *p; + int32 i; + + p = data; + + for(i=0;i<6;++i) *p++ = way->ident[i]; + GPS_Util_Put_Int(p,(int32)GPS_Math_Deg_To_Semi(way->lat)); + p+=sizeof(int32); + GPS_Util_Put_Int(p,(int32)GPS_Math_Deg_To_Semi(way->lon)); + p+=sizeof(int32); + GPS_Util_Put_Uint(p,0); + p+=sizeof(int32); + for(i=0;i<40;++i) *p++ = way->cmnt[i]; + + GPS_Util_Put_Float(p,way->dst); + p+= sizeof(float); + + *p = way->smbl; + + *len = 63; + + return; +} + + +/* @funcstatic GPS_D102_Send ******************************************** +** +** Form waypoint data string +** +** @param [w] data [UC *] string to write to +** @param [r] way [GPS_PWay] waypoint data +** @param [w] len [int32 *] packet length +** +** @return [void] +************************************************************************/ +static void GPS_D102_Send(UC *data, GPS_PWay way, int32 *len) +{ + UC *p; + int32 i; + + p = data; + + for(i=0;i<6;++i) *p++ = way->ident[i]; + GPS_Util_Put_Int(p,(int32)GPS_Math_Deg_To_Semi(way->lat)); + p+=sizeof(int32); + GPS_Util_Put_Int(p,(int32)GPS_Math_Deg_To_Semi(way->lon)); + p+=sizeof(int32); + GPS_Util_Put_Uint(p,0); + p+=sizeof(int32); + for(i=0;i<40;++i) *p++ = way->cmnt[i]; + + GPS_Util_Put_Float(p,way->dst); + p+= sizeof(float); + + GPS_Util_Put_Short(p,way->smbl); + + *len = 64; + + return; +} + + +/* @funcstatic GPS_D103_Send ******************************************* +** +** Form waypoint data string +** +** @param [w] data [UC *] string to write to +** @param [r] way [GPS_PWay] waypoint data +** @param [w] len [int32 *] packet length +** +** @return [void] +************************************************************************/ +static void GPS_D103_Send(UC *data, GPS_PWay way, int32 *len) +{ + UC *p; + int32 i; + + p = data; + + for(i=0;i<6;++i) *p++ = way->ident[i]; + GPS_Util_Put_Int(p,(int32)GPS_Math_Deg_To_Semi(way->lat)); + p+=sizeof(int32); + GPS_Util_Put_Int(p,(int32)GPS_Math_Deg_To_Semi(way->lon)); + p+=sizeof(int32); + GPS_Util_Put_Uint(p,0); + p+=sizeof(int32); + for(i=0;i<40;++i) *p++ = way->cmnt[i]; + + *p++ = way->smbl; + *p = way->dspl; + + *len = 60; + + return; +} + + +/* @funcstatic GPS_D104_Send ******************************************** +** +** Form waypoint data string +** +** @param [w] data [UC *] string to write to +** @param [r] way [GPS_PWay] waypoint data +** @param [w] len [int32 *] packet length +** +** @return [void] +************************************************************************/ +static void GPS_D104_Send(UC *data, GPS_PWay way, int32 *len) +{ + UC *p; + int32 i; + + p = data; + + for(i=0;i<6;++i) *p++ = way->ident[i]; + GPS_Util_Put_Int(p,(int32)GPS_Math_Deg_To_Semi(way->lat)); + p+=sizeof(int32); + GPS_Util_Put_Int(p,(int32)GPS_Math_Deg_To_Semi(way->lon)); + p+=sizeof(int32); + GPS_Util_Put_Uint(p,0); + p+=sizeof(int32); + for(i=0;i<40;++i) *p++ = way->cmnt[i]; + + GPS_Util_Put_Float(p,way->dst); + p+= sizeof(float); + + GPS_Util_Put_Short(p,way->smbl); + p+=sizeof(int16); + + *p = way->dspl; + + *len = 65; + + return; +} + + +/* @funcstatic GPS_D105_Send ******************************************* +** +** Form waypoint data string +** +** @param [w] data [UC *] string to write to +** @param [r] way [GPS_PWay] waypoint data +** @param [w] len [int32 *] packet length +** +** @return [void] +************************************************************************/ +static void GPS_D105_Send(UC *data, GPS_PWay way, int32 *len) +{ + UC *p; + UC *q; + + p = data; + + GPS_Util_Put_Int(p,(int32)GPS_Math_Deg_To_Semi(way->lat)); + p+=sizeof(int32); + GPS_Util_Put_Int(p,(int32)GPS_Math_Deg_To_Semi(way->lon)); + p+=sizeof(int32); + + GPS_Util_Put_Short(p,way->smbl); + p+=sizeof(int16); + + q = (UC *) way->wpt_ident; + while((*p++ = *q++)); + + + *len = p-data; + + return; +} + + +/* @funcstatic GPS_D106_Send ******************************************** +** +** Form waypoint data string +** +** @param [w] data [UC *] string to write to +** @param [r] way [GPS_PWay] waypoint data +** @param [w] len [int32 *] packet length +** +** @return [void] +************************************************************************/ +static void GPS_D106_Send(UC *data, GPS_PWay way, int32 *len) +{ + UC *p; + UC *q; + int32 i; + + p = data; + + *p++ = way->wpt_class; + for(i=0;i<13;++i) *p++ = way->subclass[i]; + GPS_Util_Put_Int(p,(int32)GPS_Math_Deg_To_Semi(way->lat)); + p+=sizeof(int32); + GPS_Util_Put_Int(p,(int32)GPS_Math_Deg_To_Semi(way->lon)); + p+=sizeof(int32); + + GPS_Util_Put_Short(p,way->smbl); + p+=sizeof(int16); + + q = (UC *) way->wpt_ident; + while((*p++ = *q++)); + q = (UC *) way->lnk_ident; + while((*p++ = *q++)); + + *len = p-data; + + return; +} + + +/* @funcstatic GPS_D107_Send ******************************************** +** +** Form waypoint data string +** +** @param [w] data [UC *] string to write to +** @param [r] way [GPS_PWay] waypoint data +** @param [w] len [int32 *] packet length +** +** @return [void] +************************************************************************/ +static void GPS_D107_Send(UC *data, GPS_PWay way, int32 *len) +{ + UC *p; + int32 i; + + p = data; + for(i=0;i<6;++i) *p++ = way->ident[i]; + GPS_Util_Put_Int(p,(int32)GPS_Math_Deg_To_Semi(way->lat)); + p+=sizeof(int32); + GPS_Util_Put_Int(p,(int32)GPS_Math_Deg_To_Semi(way->lon)); + p+=sizeof(int32); + GPS_Util_Put_Uint(p,0); + p+=sizeof(int32); + for(i=0;i<40;++i) *p++ = way->cmnt[i]; + + *p++ = way->smbl; + *p++ = way->dspl; + + GPS_Util_Put_Float(p,way->dst); + p+= sizeof(float); + + *p = way->colour; + + *len = 65; + + return; +} + + + + +/* @funcstatic GPS_D108_Send ******************************************** +** +** Form waypoint data string +** +** @param [w] data [UC *] string to write to +** @param [r] way [GPS_PWay] waypoint data +** @param [w] len [int32 *] packet length +** +** @return [void] +************************************************************************/ +static void GPS_D108_Send(UC *data, GPS_PWay way, int32 *len) +{ + UC *p; + UC *q; + + int32 i; + + p = data; + + *p++ = way->wpt_class; + *p++ = way->colour; + *p++ = way->dspl; + *p++ = 0x60; + GPS_Util_Put_Short(p,way->smbl); + p+=sizeof(int16); + for(i=0;i<18;++i) *p++ = way->subclass[i]; + GPS_Util_Put_Int(p,(int32)GPS_Math_Deg_To_Semi(way->lat)); + p+=sizeof(int32); + GPS_Util_Put_Int(p,(int32)GPS_Math_Deg_To_Semi(way->lon)); + p+=sizeof(int32); + + GPS_Util_Put_Float(p,way->alt); + p+=sizeof(float); + GPS_Util_Put_Float(p,way->dpth); + p+=sizeof(float); + GPS_Util_Put_Float(p,way->dst); + p+=sizeof(float); + + for(i=0;i<2;++i) *p++ = way->state[i]; + for(i=0;i<2;++i) *p++ = way->cc[i]; + + + q = (UC *) way->ident; + while((*p++ = *q++)); + q = (UC *) way->cmnt; + while((*p++ = *q++)); + q = (UC *) way->facility; + while((*p++ = *q++)); + q = (UC *) way->city; + while((*p++ = *q++)); + q = (UC *) way->addr; + while((*p++ = *q++)); + q = (UC *) way->cross_road; + while((*p++ = *q++)); + + *len = p-data; + + return; +} + + + + +/* @funcstatic GPS_D150_Send ******************************************** +** +** Form waypoint data string +** +** @param [w] data [UC *] string to write to +** @param [r] way [GPS_PWay] waypoint data +** @param [w] len [int32 *] packet length +** +** @return [void] +************************************************************************/ +static void GPS_D150_Send(UC *data, GPS_PWay way, int32 *len) +{ + UC *p; + int32 i; + + p = data; + + for(i=0;i<6;++i) *p++ = way->ident[i]; + for(i=0;i<2;++i) *p++ = way->cc[i]; + + if(way->wpt_class == 7) way->wpt_class = 0; + *p++ = way->wpt_class; + + GPS_Util_Put_Int(p,(int32)GPS_Math_Deg_To_Semi(way->lat)); + p+=sizeof(int32); + GPS_Util_Put_Int(p,(int32)GPS_Math_Deg_To_Semi(way->lon)); + p+=sizeof(int32); + + GPS_Util_Put_Short(p,way->alt); + p+=sizeof(int16); + + for(i=0;i<24;++i) *p++ = way->city[i]; + for(i=0;i<2;++i) *p++ = way->state[i]; + for(i=0;i<30;++i) *p++ = way->name[i]; + for(i=0;i<40;++i) *p++ = way->cmnt[i]; + + *len = 115; + + return; +} + + +/* @funcstatic GPS_D151_Send ******************************************** +** +** Form waypoint data string +** +** @param [w] data [UC *] string to write to +** @param [r] way [GPS_PWay] waypoint data +** @param [w] len [int32 *] packet length +** +** @return [void] +************************************************************************/ +static void GPS_D151_Send(UC *data, GPS_PWay way, int32 *len) +{ + UC *p; + int32 i; + + p = data; + + for(i=0;i<6;++i) *p++ = way->ident[i]; + + GPS_Util_Put_Int(p,(int32)GPS_Math_Deg_To_Semi(way->lat)); + p+=sizeof(int32); + GPS_Util_Put_Int(p,(int32)GPS_Math_Deg_To_Semi(way->lon)); + p+=sizeof(int32); + GPS_Util_Put_Uint(p,0); + p+=sizeof(int32); + for(i=0;i<40;++i) *p++ = way->cmnt[i]; + GPS_Util_Put_Float(p,way->dst); + p+=sizeof(float); + + for(i=0;i<30;++i) *p++ = way->name[i]; + for(i=0;i<24;++i) *p++ = way->city[i]; + for(i=0;i<2;++i) *p++ = way->state[i]; + + GPS_Util_Put_Short(p,way->alt); + p+=sizeof(int16); + + for(i=0;i<2;++i) *p++ = way->cc[i]; + *p++ = 0; + + if(way->wpt_class == 3) way->wpt_class = 0; + *p = way->wpt_class; + + *len = 124; + + return; +} + + + +/* @funcstatic GPS_D152_Send ******************************************** +** +** Form waypoint data string +** +** @param [w] data [UC *] string to write to +** @param [r] way [GPS_PWay] waypoint data +** @param [w] len [int32 *] packet length +** +** @return [void] +************************************************************************/ +static void GPS_D152_Send(UC *data, GPS_PWay way, int32 *len) +{ + UC *p; + int32 i; + + p = data; + + for(i=0;i<6;++i) *p++ = way->ident[i]; + + GPS_Util_Put_Int(p,(int32)GPS_Math_Deg_To_Semi(way->lat)); + p+=sizeof(int32); + GPS_Util_Put_Int(p,(int32)GPS_Math_Deg_To_Semi(way->lon)); + p+=sizeof(int32); + GPS_Util_Put_Uint(p,0); + p+=sizeof(int32); + for(i=0;i<40;++i) *p++ = way->cmnt[i]; + GPS_Util_Put_Float(p,way->dst); + p+=sizeof(float); + + for(i=0;i<30;++i) *p++ = way->name[i]; + for(i=0;i<24;++i) *p++ = way->city[i]; + for(i=0;i<2;++i) *p++ = way->state[i]; + + GPS_Util_Put_Short(p,way->alt); + p+=sizeof(int16); + + for(i=0;i<2;++i) *p++ = way->cc[i]; + *p++ = 0; + + if(way->wpt_class == 5) way->wpt_class = 0; + *p = way->wpt_class; + + *len = 124; + + return; +} + + +/* @funcstatic GPS_D154_Send ******************************************* +** +** Form waypoint data string +** +** @param [w] data [UC *] string to write to +** @param [r] way [GPS_PWay] waypoint data +** @param [w] len [int32 *] packet length +** +** @return [void] +************************************************************************/ +static void GPS_D154_Send(UC *data, GPS_PWay way, int32 *len) +{ + UC *p; + int32 i; + + p = data; + + for(i=0;i<6;++i) *p++ = way->ident[i]; + + GPS_Util_Put_Int(p,(int32)GPS_Math_Deg_To_Semi(way->lat)); + p+=sizeof(int32); + GPS_Util_Put_Int(p,(int32)GPS_Math_Deg_To_Semi(way->lon)); + p+=sizeof(int32); + GPS_Util_Put_Uint(p,0); + p+=sizeof(int32); + for(i=0;i<40;++i) *p++ = way->cmnt[i]; + GPS_Util_Put_Float(p,way->dst); + p+=sizeof(float); + + for(i=0;i<30;++i) *p++ = way->name[i]; + for(i=0;i<24;++i) *p++ = way->city[i]; + for(i=0;i<2;++i) *p++ = way->state[i]; + + GPS_Util_Put_Short(p,way->alt); + p+=sizeof(int16); + + for(i=0;i<2;++i) *p++ = way->cc[i]; + *p++ = 0; + + if(way->wpt_class == 9) way->wpt_class = 0; + *p++ = way->wpt_class; + + GPS_Util_Put_Short(p,(int16)way->smbl); + + *len = 126; + + return; +} + + + +/* @funcstatic GPS_D155_Send ******************************************* +** +** Form waypoint data string +** +** @param [w] data [UC *] string to write to +** @param [r] way [GPS_PWay] waypoint data +** @param [w] len [int32 *] packet length +** +** @return [void] +************************************************************************/ +static void GPS_D155_Send(UC *data, GPS_PWay way, int32 *len) +{ + UC *p; + int32 i; + + p = data; + + for(i=0;i<6;++i) *p++ = way->ident[i]; + + GPS_Util_Put_Int(p,(int32)GPS_Math_Deg_To_Semi(way->lat)); + p+=sizeof(int32); + GPS_Util_Put_Int(p,(int32)GPS_Math_Deg_To_Semi(way->lon)); + p+=sizeof(int32); + GPS_Util_Put_Uint(p,0); + p+=sizeof(int32); + for(i=0;i<40;++i) *p++ = way->cmnt[i]; + GPS_Util_Put_Float(p,way->dst); + p+=sizeof(float); + + for(i=0;i<30;++i) *p++ = way->name[i]; + for(i=0;i<24;++i) *p++ = way->city[i]; + for(i=0;i<2;++i) *p++ = way->state[i]; + + GPS_Util_Put_Short(p,way->alt); + p+=sizeof(int16); + + for(i=0;i<2;++i) *p++ = way->cc[i]; + *p++ = 0; + + if(way->wpt_class == 5) way->wpt_class = 0; + *p++ = way->wpt_class; + + GPS_Util_Put_Short(p,(int16)way->smbl); + p+=sizeof(int16); + + *p = way->dspl; + + *len = 127; + + return; +} + + + +/* @func GPS_A200_Get ****************************************************** +** +** Get route data from GPS +** +** @param [r] port [const char *] serial port +** @param [w] way [GPS_PWay **] waypoint array +** +** @return [int32] number of waypoint entries +************************************************************************/ +int32 GPS_A200_Get(const char *port, GPS_PWay **way) +{ + static UC data[2]; + int32 fd; + GPS_PPacket tra; + GPS_PPacket rec; + int32 n; + int32 i; + + + if(!GPS_Serial_On(port,&fd)) + return gps_errno; + + if(!(tra = GPS_Packet_New()) || !(rec = GPS_Packet_New())) + return MEMORY_ERROR; + + GPS_Util_Put_Short(data, + COMMAND_ID[gps_device_command].Cmnd_Transfer_Rte); + GPS_Make_Packet(&tra, LINK_ID[gps_link_type].Pid_Command_Data, + data,2); + if(!GPS_Write_Packet(fd,tra)) + return gps_errno; + if(!GPS_Get_Ack(fd, &tra, &rec)) + return gps_errno; + + if(!GPS_Packet_Read(fd, &rec)) + return gps_errno; + if(!GPS_Send_Ack(fd, &tra, &rec)) + return gps_errno; + + n = GPS_Util_Get_Short(rec->data); + + if(n) + if(!((*way)=(GPS_PWay *)malloc(n*sizeof(GPS_PWay)))) + { + GPS_Error("A200_Get: Insufficient memory"); + return MEMORY_ERROR; + } + + + for(i=0;itype == LINK_ID[gps_link_type].Pid_Rte_Hdr) + { + switch(gps_rte_hdr_type) + { + case pD200: + GPS_D200_Get(&((*way)[i]),rec->data); + break; + case pD201: + GPS_D201_Get(&((*way)[i]),rec->data); + break; + case pD202: + GPS_D202_Get(&((*way)[i]),rec->data); + break; + default: + GPS_Error("A200_GET: Unknown route protocol"); + return PROTOCOL_ERROR; + } + continue; + } + + if(rec->type != LINK_ID[gps_link_type].Pid_Rte_Wpt_Data) + { + GPS_Error("A200_GET: Non Pid_rte_Wpt_Data"); + return FRAMING_ERROR; + } + + (*way)[i]->isrte = 0; + (*way)[i]->islink = 0; + + switch(gps_rte_type) + { + case pD100: + GPS_D100_Get(&((*way)[i]),rec->data); + break; + case pD101: + GPS_D101_Get(&((*way)[i]),rec->data); + break; + case pD102: + GPS_D102_Get(&((*way)[i]),rec->data); + break; + case pD103: + GPS_D103_Get(&((*way)[i]),rec->data); + break; + case pD104: + GPS_D104_Get(&((*way)[i]),rec->data); + break; + case pD105: + GPS_D105_Get(&((*way)[i]),rec->data); + break; + case pD106: + GPS_D106_Get(&((*way)[i]),rec->data); + break; + case pD107: + GPS_D107_Get(&((*way)[i]),rec->data); + break; + case pD108: + GPS_D108_Get(&((*way)[i]),rec->data); + break; + case pD150: + GPS_D150_Get(&((*way)[i]),rec->data); + break; + case pD151: + GPS_D151_Get(&((*way)[i]),rec->data); + break; + case pD152: + GPS_D152_Get(&((*way)[i]),rec->data); + break; + case pD154: + GPS_D154_Get(&((*way)[i]),rec->data); + break; + case pD155: + GPS_D155_Get(&((*way)[i]),rec->data); + break; + default: + GPS_Error("A200_GET: Unknown route protocol"); + return PROTOCOL_ERROR; + } + (*way)[i-1]->prot = (*way)[i]->prot; + } + + if(!GPS_Packet_Read(fd, &rec)) + return gps_errno; + + if(!GPS_Send_Ack(fd, &tra, &rec)) + return gps_errno; + + if(rec->type != LINK_ID[gps_link_type].Pid_Xfer_Cmplt) + { + GPS_Error("A200_GET: Error transferring routes"); + return FRAMING_ERROR; + } + + if(i != n) + { + GPS_Error("A200_GET: Route entry number mismatch"); + return FRAMING_ERROR; + } + + GPS_Packet_Del(&tra); + GPS_Packet_Del(&rec); + + + if(!GPS_Serial_Off(port, fd)) + return gps_errno; + + return n; +} + + + +/* @func GPS_A201_Get ****************************************************** +** +** Get route data from GPS +** +** @param [r] port [const char *] serial port +** @param [w] way [GPS_PWay **] waypoint array +** +** @return [int32] number of waypoint entries +************************************************************************/ +int32 GPS_A201_Get(const char *port, GPS_PWay **way) +{ + static UC data[2]; + int32 fd; + GPS_PPacket tra; + GPS_PPacket rec; + int32 n; + int32 i; + + + if(!GPS_Serial_On(port,&fd)) + return gps_errno; + + if(!(tra = GPS_Packet_New()) || !(rec = GPS_Packet_New())) + return MEMORY_ERROR; + + GPS_Util_Put_Short(data, + COMMAND_ID[gps_device_command].Cmnd_Transfer_Rte); + GPS_Make_Packet(&tra, LINK_ID[gps_link_type].Pid_Command_Data, + data,2); + if(!GPS_Write_Packet(fd,tra)) + return gps_errno; + if(!GPS_Get_Ack(fd, &tra, &rec)) + return gps_errno; + + if(!GPS_Packet_Read(fd, &rec)) + return gps_errno; + if(!GPS_Send_Ack(fd, &tra, &rec)) + return gps_errno; + + n = GPS_Util_Get_Short(rec->data); + + if(n) + if(!((*way)=(GPS_PWay *)malloc(n*sizeof(GPS_PWay)))) + { + GPS_Error("A201_Get: Insufficient memory"); + return MEMORY_ERROR; + } + + + for(i=0;itype == LINK_ID[gps_link_type].Pid_Rte_Hdr) + { + switch(gps_rte_hdr_type) + { + case pD200: + GPS_D200_Get(&((*way)[i]),rec->data); + break; + case pD201: + GPS_D201_Get(&((*way)[i]),rec->data); + break; + case pD202: + GPS_D202_Get(&((*way)[i]),rec->data); + break; + default: + GPS_Error("A201_GET: Unknown route protocol"); + return PROTOCOL_ERROR; + } + (*way)[i]->islink = 0; + continue; + } + + + if(rec->type == LINK_ID[gps_link_type].Pid_Rte_Link_Data) + { + switch(gps_rte_link_type) + { + case pD210: + GPS_D210_Get(&((*way)[i]),rec->data); + break; + default: + GPS_Error("A201_GET: Unknown route protocol"); + return PROTOCOL_ERROR; + } + (*way)[i]->isrte = 0; + (*way)[i]->islink = 1; + continue; + } + + if(rec->type != LINK_ID[gps_link_type].Pid_Rte_Wpt_Data) + { + GPS_Error("A200_GET: Non Pid_rte_Wpt_Data"); + return FRAMING_ERROR; + } + + (*way)[i]->isrte = 0; + (*way)[i]->islink = 0; + + switch(gps_rte_type) + { + case pD100: + GPS_D100_Get(&((*way)[i]),rec->data); + break; + case pD101: + GPS_D101_Get(&((*way)[i]),rec->data); + break; + case pD102: + GPS_D102_Get(&((*way)[i]),rec->data); + break; + case pD103: + GPS_D103_Get(&((*way)[i]),rec->data); + break; + case pD104: + GPS_D104_Get(&((*way)[i]),rec->data); + break; + case pD105: + GPS_D105_Get(&((*way)[i]),rec->data); + break; + case pD106: + GPS_D106_Get(&((*way)[i]),rec->data); + break; + case pD107: + GPS_D107_Get(&((*way)[i]),rec->data); + break; + case pD108: + GPS_D108_Get(&((*way)[i]),rec->data); + break; + case pD150: + GPS_D150_Get(&((*way)[i]),rec->data); + break; + case pD151: + GPS_D151_Get(&((*way)[i]),rec->data); + break; + case pD152: + GPS_D152_Get(&((*way)[i]),rec->data); + break; + case pD154: + GPS_D154_Get(&((*way)[i]),rec->data); + break; + case pD155: + GPS_D155_Get(&((*way)[i]),rec->data); + break; + default: + GPS_Error("A200_GET: Unknown route protocol"); + return PROTOCOL_ERROR; + } + (*way)[i-1]->prot = (*way)[i]->prot; + } + + if(!GPS_Packet_Read(fd, &rec)) + return gps_errno; + + if(!GPS_Send_Ack(fd, &tra, &rec)) + return gps_errno; + + if(rec->type != LINK_ID[gps_link_type].Pid_Xfer_Cmplt) + { + GPS_Error("A200_GET: Error transferring routes"); + return FRAMING_ERROR; + } + + if(i != n) + { + GPS_Error("A200_GET: Route entry number mismatch"); + return FRAMING_ERROR; + } + + GPS_Packet_Del(&tra); + GPS_Packet_Del(&rec); + + + if(!GPS_Serial_Off(port, fd)) + return gps_errno; + + return n; +} + + + +/* @func GPS_A200_Send ************************************************** +** +** Send routes to GPS +** +** @param [r] port [const char *] serial port +** @param [r] trk [GPS_PWay *] waypoint array +** @param [r] n [int32] number of waypoint entries +** +** @return [int32] success +************************************************************************/ +int32 GPS_A200_Send(const char *port, GPS_PWay *way, int32 n) +{ + UC data[GPS_ARB_LEN]; + int32 fd; + GPS_PPacket tra; + GPS_PPacket rec; + int32 i; + int32 len; + UC method; + + if(!GPS_Serial_On(port,&fd)) + return gps_errno; + + if(!(tra = GPS_Packet_New()) || !(rec = GPS_Packet_New())) + return MEMORY_ERROR; + + GPS_Util_Put_Short(data,n); + GPS_Make_Packet(&tra, LINK_ID[gps_link_type].Pid_Records, + data,2); + if(!GPS_Write_Packet(fd,tra)) + return gps_errno; + if(!GPS_Get_Ack(fd, &tra, &rec)) + { + GPS_Error("A200_Send: Route start data not acknowledged"); + return FRAMING_ERROR; + } + + + for(i=0;iisrte) + { + method = LINK_ID[gps_link_type].Pid_Rte_Hdr; + + switch(gps_rte_hdr_type) + { + case pD200: + GPS_D200_Send(data,way[i],&len); + break; + case pD201: + GPS_D201_Send(data,way[i],&len); + break; + case pD202: + GPS_D202_Send(data,way[i],&len); + break; + default: + GPS_Error("A200_Send: Unknown route protocol"); + return PROTOCOL_ERROR; + } + } + else + { + method = LINK_ID[gps_link_type].Pid_Rte_Wpt_Data; + + switch(gps_rte_type) + { + case pD100: + GPS_D100_Send(data,way[i],&len); + break; + case pD101: + GPS_D101_Send(data,way[i],&len); + break; + case pD102: + GPS_D102_Send(data,way[i],&len); + break; + case pD103: + GPS_D103_Send(data,way[i],&len); + break; + case pD104: + GPS_D104_Send(data,way[i],&len); + break; + case pD105: + GPS_D105_Send(data,way[i],&len); + break; + case pD106: + GPS_D106_Send(data,way[i],&len); + break; + case pD107: + GPS_D107_Send(data,way[i],&len); + break; + case pD108: + GPS_D108_Send(data,way[i],&len); + break; + case pD150: + GPS_D150_Send(data,way[i],&len); + break; + case pD151: + GPS_D151_Send(data,way[i],&len); + break; + case pD152: + GPS_D152_Send(data,way[i],&len); + break; + case pD154: + GPS_D154_Send(data,way[i],&len); + break; + case pD155: + GPS_D155_Send(data,way[i],&len); + break; + default: + GPS_Error("A200_Send: Unknown route protocol"); + return PROTOCOL_ERROR; + } + } + + + GPS_Make_Packet(&tra, method, data,len); + + if(!GPS_Write_Packet(fd,tra)) + return gps_errno; + + if(!GPS_Get_Ack(fd, &tra, &rec)) + { + GPS_Error("A200_Send: Route packet not acknowledged"); + return FRAMING_ERROR; + } + } + + GPS_Util_Put_Short(data,COMMAND_ID[gps_device_command].Cmnd_Transfer_Wpt); + GPS_Make_Packet(&tra, LINK_ID[gps_link_type].Pid_Xfer_Cmplt, + data,2); + if(!GPS_Write_Packet(fd,tra)) + return gps_errno; + if(!GPS_Get_Ack(fd, &tra, &rec)) + { + GPS_Error("A200_Send: Route complete data not acknowledged"); + return FRAMING_ERROR; + } + + GPS_Packet_Del(&tra); + GPS_Packet_Del(&rec); + + if(!GPS_Serial_Off(port, fd)) + return gps_errno; + + return 1; +} + + + +/* @func GPS_A201_Send ************************************************** +** +** Send routes to GPS +** +** @param [r] port [const char *] serial port +** @param [r] trk [GPS_PWay *] waypoint array +** @param [r] n [int32] number of waypoint entries +** +** @return [int32] success +************************************************************************/ +int32 GPS_A201_Send(const char *port, GPS_PWay *way, int32 n) +{ + UC data[GPS_ARB_LEN]; + int32 fd; + GPS_PPacket tra; + GPS_PPacket rec; + int32 i; + int32 len; + UC method; + + if(!GPS_Serial_On(port,&fd)) + return gps_errno; + + if(!(tra = GPS_Packet_New()) || !(rec = GPS_Packet_New())) + return MEMORY_ERROR; + + GPS_Util_Put_Short(data,n); + GPS_Make_Packet(&tra, LINK_ID[gps_link_type].Pid_Records, + data,2); + if(!GPS_Write_Packet(fd,tra)) + return gps_errno; + if(!GPS_Get_Ack(fd, &tra, &rec)) + { + GPS_Error("A200_Send: Route start data not acknowledged"); + return FRAMING_ERROR; + } + + + for(i=0;iisrte) + { + method = LINK_ID[gps_link_type].Pid_Rte_Hdr; + + switch(gps_rte_hdr_type) + { + case pD200: + GPS_D200_Send(data,way[i],&len); + break; + case pD201: + GPS_D201_Send(data,way[i],&len); + break; + case pD202: + GPS_D202_Send(data,way[i],&len); + break; + default: + GPS_Error("A200_Send: Unknown route protocol"); + return PROTOCOL_ERROR; + } + } + else if(way[i]->islink) + { + method = LINK_ID[gps_link_type].Pid_Rte_Link_Data; + + switch(gps_rte_link_type) + { + case pD210: + GPS_D210_Send(data,way[i],&len); + break; + default: + GPS_Error("A201_Send: Unknown route protocol"); + return PROTOCOL_ERROR; + } + } + else + { + method = LINK_ID[gps_link_type].Pid_Rte_Wpt_Data; + + switch(gps_rte_type) + { + case pD100: + GPS_D100_Send(data,way[i],&len); + break; + case pD101: + GPS_D101_Send(data,way[i],&len); + break; + case pD102: + GPS_D102_Send(data,way[i],&len); + break; + case pD103: + GPS_D103_Send(data,way[i],&len); + break; + case pD104: + GPS_D104_Send(data,way[i],&len); + break; + case pD105: + GPS_D105_Send(data,way[i],&len); + break; + case pD106: + GPS_D106_Send(data,way[i],&len); + break; + case pD107: + GPS_D107_Send(data,way[i],&len); + break; + case pD108: + GPS_D108_Send(data,way[i],&len); + break; + case pD150: + GPS_D150_Send(data,way[i],&len); + break; + case pD151: + GPS_D151_Send(data,way[i],&len); + break; + case pD152: + GPS_D152_Send(data,way[i],&len); + break; + case pD154: + GPS_D154_Send(data,way[i],&len); + break; + case pD155: + GPS_D155_Send(data,way[i],&len); + break; + default: + GPS_Error("A200_Send: Unknown route protocol"); + return PROTOCOL_ERROR; + } + } + + + GPS_Make_Packet(&tra, method, data,len); + + if(!GPS_Write_Packet(fd,tra)) + return gps_errno; + + if(!GPS_Get_Ack(fd, &tra, &rec)) + { + GPS_Error("A200_Send: Route packet not acknowledged"); + return FRAMING_ERROR; + } + } + + GPS_Util_Put_Short(data,COMMAND_ID[gps_device_command].Cmnd_Transfer_Wpt); + GPS_Make_Packet(&tra, LINK_ID[gps_link_type].Pid_Xfer_Cmplt, + data,2); + if(!GPS_Write_Packet(fd,tra)) + return gps_errno; + if(!GPS_Get_Ack(fd, &tra, &rec)) + { + GPS_Error("A200_Send: Route complete data not acknowledged"); + return FRAMING_ERROR; + } + + GPS_Packet_Del(&tra); + GPS_Packet_Del(&rec); + + if(!GPS_Serial_Off(port, fd)) + return gps_errno; + + return 1; +} + + + + + +/* @funcstatic GPS_D200_Get ******************************************** +** +** Get route header data +** +** @param [w] way [GPS_PWay *] waypoint array +** @param [r] s [UC *] packet data +** +** @return [void] +************************************************************************/ +static void GPS_D200_Get(GPS_PWay *way, UC *s) +{ + (*way)->rte_prot = 200; + (*way)->rte_num = *s; + (*way)->isrte = 1; + + return; +} + + + +/* @funcstatic GPS_D201_Get ******************************************* +** +** Get route header data +** +** @param [w] way [GPS_PWay *] waypoint array +** @param [r] s [UC *] packet data +** +** @return [void] +************************************************************************/ +static void GPS_D201_Get(GPS_PWay *way, UC *s) +{ + UC *p; + int32 i; + + p=s; + + (*way)->rte_prot = 201; + (*way)->rte_num = *p++; + (*way)->isrte = 1; + for(i=0;i<20;++i) (*way)->rte_cmnt[i] = *p++; + + return; +} + + + +/* @funcstatic GPS_D202_Get ******************************************** +** +** Get route header data +** +** @param [w] way [GPS_PWay *] waypoint array +** @param [r] s [UC *] packet data +** +** @return [void] +************************************************************************/ +static void GPS_D202_Get(GPS_PWay *way, UC *s) +{ + UC *p; + UC *q; + + p=s; + + (*way)->rte_prot = 201; + (*way)->rte_num = *p++; + (*way)->isrte = 1; + q = (UC *) (*way)->rte_ident; + while((*q++=*p++)); + + return; +} + + + +/* @funcstatic GPS_D210_Get ******************************************** +** +** Get route link data +** +** @param [w] way [GPS_PWay *] waypoint array +** @param [r] s [UC *] packet data +** +** @return [void] +************************************************************************/ +static void GPS_D210_Get(GPS_PWay *way, UC *s) +{ + UC *p; + UC *q; + int32 i; + + p=s; + + (*way)->rte_link_class = GPS_Util_Get_Short(p); + p+=sizeof(int16); + for(i=0;i<18;++i) (*way)->rte_link_subclass[i] = *p++; + q = (UC *) (*way)->rte_link_ident; + while((*q++=*p++)); + + return; +} + + + +/* @funcstatic GPS_D200_Send ******************************************* +** +** Form route header data string +** +** @param [w] data [UC *] string to write to +** @param [r] way [GPS_PWay] waypoint data +** @param [w] len [int32 *] packet length +** +** @return [void] +************************************************************************/ +static void GPS_D200_Send(UC *data, GPS_PWay way, int32 *len) +{ + + *data = way->rte_num; + *len = 1; + + return; +} + + + +/* @funcstatic GPS_D201_Send ******************************************* +** +** Form route header data string +** +** @param [w] data [UC *] string to write to +** @param [r] way [GPS_PWay] waypoint data +** @param [w] len [int32 *] packet length +** +** @return [void] +************************************************************************/ +static void GPS_D201_Send(UC *data, GPS_PWay way, int32 *len) +{ + UC *p; + int32 i; + + p = data; + + *p++ = way->rte_num; + for(i=0;i<20;++i) *p++ = way->rte_cmnt[i]; + *len = 21; + + return; +} + + + +/* @funcstatic GPS_D202_Send ******************************************** +** +** Form route header data string +** +** @param [w] data [UC *] string to write to +** @param [r] way [GPS_PWay] waypoint data +** @param [w] len [int32 *] packet length +** +** @return [void] +************************************************************************/ +static void GPS_D202_Send(UC *data, GPS_PWay way, int32 *len) +{ + UC *p; + UC *q; + + p = data; + q = (UC *) way->rte_ident; + + while((*p++ = *q++)); + + *len = p-data; + + return; +} + + + +/* @funcstatic GPS_D210_Send ******************************************** +** +** Form route link data string +** +** @param [w] data [UC *] string to write to +** @param [r] way [GPS_PWay] waypoint data +** @param [w] len [int32 *] packet length +** +** @return [void] +************************************************************************/ +static void GPS_D210_Send(UC *data, GPS_PWay way, int32 *len) +{ + UC *p; + UC *q; + int32 i; + + p = data; + + GPS_Util_Put_Short(p,way->rte_link_class); + p+=sizeof(int16); + for(i=0;i<18;++i) *p++ = way->rte_link_subclass[i]; + + q = (UC *) way->rte_link_ident; + while((*p++ = *q++)); + + *len = p-data; + + return; +} + + + +/* @func GPS_A300_Get ****************************************************** +** +** Get track data from GPS +** +** @param [r] port [const char *] serial port +** @param [w] trk [GPS_PTrack **] track array +** +** @return [int32] number of track entries +************************************************************************/ +int32 GPS_A300_Get(const char *port, GPS_PTrack **trk) +{ + static UC data[2]; + int32 fd; + GPS_PPacket tra; + GPS_PPacket rec; + int32 n; + int32 i; + int32 ret; + + + if(gps_trk_transfer == -1) + return GPS_UNSUPPORTED; + + /* Only those GPS' with L001 can send track data */ + if(!LINK_ID[gps_link_type].Pid_Trk_Data) + { + GPS_Warning("A300 protocol unsupported"); + return GPS_UNSUPPORTED; + } + + if(!GPS_Serial_On(port, &fd)) + return gps_errno; + + if(!(tra = GPS_Packet_New()) || !(rec = GPS_Packet_New())) + return MEMORY_ERROR; + + GPS_Util_Put_Short(data, + COMMAND_ID[gps_device_command].Cmnd_Transfer_Trk); + GPS_Make_Packet(&tra, LINK_ID[gps_link_type].Pid_Command_Data, + data,2); + if(!GPS_Write_Packet(fd,tra)) + return gps_errno; + if(!GPS_Get_Ack(fd, &tra, &rec)) + return gps_errno; + if(!GPS_Packet_Read(fd, &rec)) + return gps_errno; + if(!GPS_Send_Ack(fd, &tra, &rec)) + return gps_errno; + + + n = GPS_Util_Get_Short(rec->data); + + if(n) + if(!((*trk)=(GPS_PTrack *)malloc(n*sizeof(GPS_PTrack)))) + { + GPS_Error("A300_Get: Insufficient memory"); + return MEMORY_ERROR; + } + for(i=0;idata); + + if(n) + if(!((*trk)=(GPS_PTrack *)malloc(n*sizeof(GPS_PTrack)))) + { + GPS_Error("A301_Get: Insufficient memory"); + return MEMORY_ERROR; + } + for(i=0;itype == LINK_ID[gps_link_type].Pid_Trk_Hdr) + { + switch(gps_trk_hdr_type) + { + case pD310: + GPS_D310_Get(&((*trk)[i]),rec->data); + break; + default: + GPS_Error("A301_Get: Unknown track protocol"); + return PROTOCOL_ERROR; + } + (*trk)[i]->ishdr = 1; + continue; + } + + if(rec->type != LINK_ID[gps_link_type].Pid_Trk_Data) + { + GPS_Error("A301_Get: Non-Pid_Trk_Data"); + return FRAMING_ERROR; + } + + (*trk)[i]->ishdr = 0; + + switch(gps_trk_type) + { + case pD300: + GPS_D300b_Get(&((*trk)[i]),rec->data); + break; + case pD301: + GPS_D301b_Get(&((*trk)[i]),rec->data); + break; + default: + GPS_Error("A301_GET: Unknown track protocol"); + return PROTOCOL_ERROR; + } + } + + if(!GPS_Packet_Read(fd, &rec)) + return gps_errno; + if(!GPS_Send_Ack(fd, &tra, &rec)) + return gps_errno; + + if(rec->type != LINK_ID[gps_link_type].Pid_Xfer_Cmplt) + { + GPS_Error("A301_Get: Error transferring tracks"); + return FRAMING_ERROR; + } + + if(i != n) + { + GPS_Error("A301_GET: Track entry number mismatch"); + return FRAMING_ERROR; + } + + GPS_Packet_Del(&tra); + GPS_Packet_Del(&rec); + + if(!GPS_Serial_Off(port, fd)) + return gps_errno; + + return n; +} + + + + + +/* @func GPS_A300_Send ************************************************** +** +** Send track log to GPS +** +** @param [r] port [const char *] serial port +** @param [r] trk [GPS_PTrack *] track array +** @param [r] n [int32] number of track entries +** +** @return [int32] success +************************************************************************/ +int32 GPS_A300_Send(const char *port, GPS_PTrack *trk, int32 n) +{ + UC data[GPS_ARB_LEN]; + int32 fd; + GPS_PPacket tra; + GPS_PPacket rec; + int32 i; + int32 len; + + if(gps_trk_transfer == -1) + return GPS_UNSUPPORTED; + + /* Only those GPS' with L001 can send track data */ + if(!LINK_ID[gps_link_type].Pid_Trk_Data) + { + GPS_Warning("A300 protocol unsupported"); + return GPS_UNSUPPORTED; + } + + if(!GPS_Serial_On(port, &fd)) + return gps_errno; + + if(!(tra = GPS_Packet_New()) || !(rec = GPS_Packet_New())) + return MEMORY_ERROR; + + GPS_Util_Put_Short(data,n); + GPS_Make_Packet(&tra, LINK_ID[gps_link_type].Pid_Records, + data,2); + if(!GPS_Write_Packet(fd,tra)) + return gps_errno; + if(!GPS_Get_Ack(fd, &tra, &rec)) + { + GPS_Error("A300_Send: Track start data not acknowledged"); + return FRAMING_ERROR; + } + + for(i=0;iishdr) + { + method = LINK_ID[gps_link_type].Pid_Trk_Hdr; + + switch(gps_trk_hdr_type) + { + case pD310: + GPS_D310_Send(data,trk[i],&len); + break; + default: + GPS_Error("A301_Send: Unknown track protocol"); + return PROTOCOL_ERROR; + } + } + else + { + method = LINK_ID[gps_link_type].Pid_Trk_Data; + + switch(gps_trk_type) + { + case pD300: + GPS_D300_Send(data,trk[i]); + len = 13; + break; + case pD301: + GPS_D301_Send(data,trk[i]); + len = 21; + break; + default: + GPS_Error("A301_Send: Unknown track protocol"); + return PROTOCOL_ERROR; + } + } + + + GPS_Make_Packet(&tra, method, data,len); + + if(!GPS_Write_Packet(fd,tra)) + return gps_errno; + + if(!GPS_Get_Ack(fd, &tra, &rec)) + { + GPS_Error("A301_Send: Track packet not acknowledgedn"); + return FRAMING_ERROR; + } + } + + + GPS_Util_Put_Short(data,COMMAND_ID[gps_device_command].Cmnd_Transfer_Trk); + GPS_Make_Packet(&tra, LINK_ID[gps_link_type].Pid_Xfer_Cmplt, + data,2); + if(!GPS_Write_Packet(fd,tra)) + return gps_errno; + if(!GPS_Get_Ack(fd, &tra, &rec)) + { + GPS_Error("A301_Send: Track complete data not acknowledged"); + return FRAMING_ERROR; + } + + GPS_Packet_Del(&tra); + GPS_Packet_Del(&rec); + + if(!GPS_Serial_Off(port, fd)) + return gps_errno; + + return 1; +} + + + +/* @func GPS_D300_Get ****************************************************** +** +** Get track data +** +** @param [w] trk [GPS_PTrack *] track array +** @param [r] entries [int32] number of packets to receive +** @param [r] fd [int32] file descriptor +** +** @return [int32] number of entries read +************************************************************************/ +int32 GPS_D300_Get(GPS_PTrack *trk, int32 entries, int32 fd) +{ + GPS_PPacket tra; + GPS_PPacket rec; + int32 i; + + if(!(tra = GPS_Packet_New()) || !(rec = GPS_Packet_New())) + return MEMORY_ERROR; + + + for(i=0;idata, &trk[i]); + } + + + if(!GPS_Packet_Read(fd, &rec)) + return gps_errno; + if(!GPS_Send_Ack(fd, &tra, &rec)) + return gps_errno; + + + if(rec->type != LINK_ID[gps_link_type].Pid_Xfer_Cmplt) + { + GPS_Error("D300_GET: Error transferring track log"); + return FRAMING_ERROR; + } + + GPS_Packet_Del(&tra); + GPS_Packet_Del(&rec); + + return i; +} + + + +/* @func GPS_D300b_Get ****************************************************** +** +** Get track data (A301 protocol) +** +** @param [w] trk [GPS_PTrack *] track +** @param [r] data [UC *] packet data +** +** @return [void] +************************************************************************/ +void GPS_D300b_Get(GPS_PTrack *trk, UC *data) +{ + + GPS_A300_Translate(data, trk); + return; +} + + + +/* @func GPS_D301b_Get ****************************************************** +** +** Get track data (A301 protocol) +** +** @param [w] trk [GPS_PTrack *] track +** @param [r] data [UC *] packet data +** +** @return [void] +************************************************************************/ +void GPS_D301b_Get(GPS_PTrack *trk, UC *data) +{ + UC *p; + uint32 t; + + p=data; + + (*trk)->lat = GPS_Math_Semi_To_Deg(GPS_Util_Get_Int(p)); + p+=sizeof(int32); + + (*trk)->lon = GPS_Math_Semi_To_Deg(GPS_Util_Get_Int(p)); + p+=sizeof(int32); + + t = GPS_Util_Get_Uint(p); + if(!t || t==0x7fffffff || t==0xffffffff) + (*trk)->Time=0; + else + (*trk)->Time = GPS_Math_Gtime_To_Utime((time_t)t); + p+=sizeof(uint32); + + (*trk)->alt = GPS_Util_Get_Float(p); + p+=sizeof(float); + + (*trk)->dpth = GPS_Util_Get_Float(p); + p+=sizeof(float); + + (*trk)->tnew = *p; + + return; +} + + + +/* @func GPS_D310_Get ****************************************************** +** +** Get track header data (A301 protocol) +** +** @param [w] trk [GPS_PTrack *] track +** @param [r] s [UC *] packet data +** +** @return [void] +************************************************************************/ +void GPS_D310_Get(GPS_PTrack *trk, UC *s) +{ + UC *p; + UC *q; + + p=s; + + (*trk)->dspl = *p++; + (*trk)->colour = *p++; + + q = (UC *) (*trk)->trk_ident; + + while((*q++ = *p++)); + + return; +} + + + +/* @func GPS_D300_Send ************************************************** +** +** Form track data string +** +** @param [w] data [UC *] string to write to +** @param [r] trk [GPS_PTrack] track data +** +** @return [void] +************************************************************************/ +void GPS_D300_Send(UC *data, GPS_PTrack trk) +{ + UC *p; + + p = data; + GPS_A300_Encode(p,trk); + + return; +} + + + +/* @func GPS_D301_Send ************************************************** +** +** Form track data string +** +** @param [w] data [UC *] string to write to +** @param [r] trk [GPS_PTrack] track data +** +** @return [void] +************************************************************************/ +void GPS_D301_Send(UC *data, GPS_PTrack trk) +{ + UC *p; + + p = data; + GPS_A300_Encode(p,trk); + p = data+12; + + GPS_Util_Put_Float(p,trk->alt); + p+=sizeof(float); + GPS_Util_Put_Float(p,trk->dpth); + p+=sizeof(float); + + *p = trk->tnew; + + return; +} + + + +/* @func GPS_D310_Send ************************************************** +** +** Form track header data string +** +** @param [w] data [UC *] string to write to +** @param [r] trk [GPS_PTrack] track data +** @param [w] len [int32 *] length of data +** +** @return [void] +************************************************************************/ +void GPS_D310_Send(UC *data, GPS_PTrack trk, int32 *len) +{ + UC *p; + UC *q; + + p = data; + + *p++ = trk->dspl; + *p++ = trk->colour; + + q = (UC *) trk->trk_ident; + while((*p++ = *q++)); + + *len = p-data; + + return; +} + + +/* @funcstatic GPS_A300_Translate *************************************** +** +** Translate track packet to track structure +** +** @param [r] s [const UC *] track packet data +** @param [w] trk [GPS_PTrack *] track entry pointer +** +** @return [void] +************************************************************************/ +static void GPS_A300_Translate(UC *s, GPS_PTrack *trk) +{ + UC *p; + uint32 t; + + p=s; + + (*trk)->lat = GPS_Math_Semi_To_Deg(GPS_Util_Get_Int(p)); + p+=sizeof(int32); + + (*trk)->lon = GPS_Math_Semi_To_Deg(GPS_Util_Get_Int(p)); + p+=sizeof(int32); + + t = GPS_Util_Get_Uint(p); + if(!t || t==0x7fffffff || t==0xffffffff) + (*trk)->Time=0; + else + (*trk)->Time = GPS_Math_Gtime_To_Utime((time_t)t); + p+=sizeof(uint32); + + (*trk)->tnew = *p; + + return; +} + + + +/* @funcstatic GPS_A300_Encode *************************************** +** +** Encode track structure to track packet +** +** @param [w] s [UC *] string to write to +** @param [r] trk [GPS_PTrack] track entry +** +** @return [void] +************************************************************************/ +static void GPS_A300_Encode(UC *s, GPS_PTrack trk) +{ + UC *p; + + p=s; + + GPS_Util_Put_Int(p,GPS_Math_Deg_To_Semi(trk->lat)); + p+=sizeof(int32); + + GPS_Util_Put_Int(p,GPS_Math_Deg_To_Semi(trk->lon)); + p+=sizeof(int32); + + GPS_Util_Put_Uint(p,GPS_Math_Utime_To_Gtime(trk->Time)); + p+=sizeof(uint32); + + *p = (UC) trk->tnew; + + return; +} + + + +/* @func GPS_A400_Get ************************************************** +** +** Get proximity waypoint data from GPS +** +** @param [r] port [const char *] serial port +** @param [w] way [GPS_PWay **] waypoint array +** +** @return [int32] number of waypoint entries +************************************************************************/ +int32 GPS_A400_Get(const char *port, GPS_PWay **way) +{ + static UC data[2]; + int32 fd; + GPS_PPacket tra; + GPS_PPacket rec; + int32 n; + int32 i; + + if(gps_prx_waypt_transfer == -1) + return GPS_UNSUPPORTED; + + + if(!GPS_Serial_On(port, &fd)) + return gps_errno; + + if(!(tra = GPS_Packet_New()) || !(rec = GPS_Packet_New())) + return MEMORY_ERROR; + + + GPS_Util_Put_Short(data, + COMMAND_ID[gps_device_command].Cmnd_Transfer_Prx); + GPS_Make_Packet(&tra, LINK_ID[gps_link_type].Pid_Command_Data, + data,2); + if(!GPS_Write_Packet(fd,tra)) + return gps_errno; + if(!GPS_Get_Ack(fd, &tra, &rec)) + return gps_errno; + + if(!GPS_Serial_Chars_Ready(fd)) + { + GPS_Warning("A400 (ppx) protocol not supported"); + GPS_Packet_Del(&rec); + GPS_Packet_Del(&tra); + + if(!GPS_Serial_Off(port, fd)) + return gps_errno; + + return GPS_UNSUPPORTED; + } + + if(!GPS_Packet_Read(fd, &rec)) + return gps_errno; + + if(!GPS_Send_Ack(fd, &tra, &rec)) + return gps_errno; + + n = GPS_Util_Get_Short(rec->data); + + if(n) + if(!((*way)=(GPS_PWay *)malloc(n*sizeof(GPS_PWay)))) + { + GPS_Error("A400_Get: Insufficient memory"); + return MEMORY_ERROR; + } + + + for(i=0;idata); + break; + case pD101: + GPS_D101_Get(&((*way)[i]),rec->data); + break; + case pD102: + GPS_D102_Get(&((*way)[i]),rec->data); + break; + case pD403: + GPS_D403_Get(&((*way)[i]),rec->data); + break; + case pD104: + GPS_D104_Get(&((*way)[i]),rec->data); + break; + case pD105: + GPS_D105_Get(&((*way)[i]),rec->data); + break; + case pD106: + GPS_D106_Get(&((*way)[i]),rec->data); + break; + case pD107: + GPS_D107_Get(&((*way)[i]),rec->data); + break; + case pD108: + GPS_D108_Get(&((*way)[i]),rec->data); + break; + case pD450: + GPS_D450_Get(&((*way)[i]),rec->data); + break; + case pD151: + GPS_D151_Get(&((*way)[i]),rec->data); + break; + case pD152: + GPS_D152_Get(&((*way)[i]),rec->data); + break; + case pD154: + GPS_D154_Get(&((*way)[i]),rec->data); + break; + case pD155: + GPS_D155_Get(&((*way)[i]),rec->data); + break; + default: + GPS_Error("A400_GET: Unknown prx waypoint protocol"); + return PROTOCOL_ERROR; + } + } + + if(!GPS_Packet_Read(fd, &rec)) + return gps_errno; + if(!GPS_Send_Ack(fd, &tra, &rec)) + return gps_errno; + + if(rec->type != LINK_ID[gps_link_type].Pid_Xfer_Cmplt) + { + GPS_Error("A400_GET: Error transferring prx waypoints"); + return FRAMING_ERROR; + } + + if(i != n) + { + GPS_Error("A400_GET: Prx waypoint entry number mismatch"); + return FRAMING_ERROR; + } + + + GPS_Packet_Del(&tra); + GPS_Packet_Del(&rec); + + if(!GPS_Serial_Off(port, fd)) + return gps_errno; + + return n; +} + + + +/* @func GPS_A400_Send ************************************************** +** +** Send proximity waypoints to GPS +** +** @param [r] port [const char *] serial port +** @param [r] trk [GPS_PWay *] waypoint array +** @param [r] n [int32] number of waypoint entries +** +** @return [int32] success +************************************************************************/ +int32 GPS_A400_Send(const char *port, GPS_PWay *way, int32 n) +{ + UC data[GPS_ARB_LEN]; + int32 fd; + GPS_PPacket tra; + GPS_PPacket rec; + int32 i; + int32 len; + + if(gps_prx_waypt_transfer == -1) + return GPS_UNSUPPORTED; + + if(!GPS_Serial_On(port, &fd)) + return gps_errno; + + if(!(tra = GPS_Packet_New()) || !(rec = GPS_Packet_New())) + return MEMORY_ERROR; + + + GPS_Util_Put_Short(data,n); + GPS_Make_Packet(&tra, LINK_ID[gps_link_type].Pid_Records, + data,2); + if(!GPS_Write_Packet(fd,tra)) + return gps_errno; + if(!GPS_Get_Ack(fd, &tra, &rec)) + { + GPS_Error("A400_Send: Prx start data not acknowledgedn"); + return FRAMING_ERROR; + } + + + for(i=0;iprot = 400; + for(i=0;i<6;++i) (*way)->ident[i] = *p++; + + (*way)->lat = GPS_Math_Semi_To_Deg(GPS_Util_Get_Int(p)); + p+=sizeof(int32); + + (*way)->lon = GPS_Math_Semi_To_Deg(GPS_Util_Get_Int(p)); + p+=sizeof(int32); + + p+=sizeof(int32); + + for(i=0;i<40;++i) (*way)->cmnt[i] = *p++; + + (*way)->dst=GPS_Util_Get_Float(p); + + + return; +} + + +/* @funcstatic GPS_D403_Get ******************************************** +** +** Get proximity waypoint data +** +** @param [w] way [GPS_PWay *] waypoint array +** @param [r] s [UC *] packet data +** +** @return [void] +************************************************************************/ +static void GPS_D403_Get(GPS_PWay *way, UC *s) +{ + UC *p; + int32 i; + + p=s; + + (*way)->prot = 403; + for(i=0;i<6;++i) (*way)->ident[i] = *p++; + + (*way)->lat = GPS_Math_Semi_To_Deg(GPS_Util_Get_Int(p)); + p+=sizeof(int32); + + (*way)->lon = GPS_Math_Semi_To_Deg(GPS_Util_Get_Int(p)); + p+=sizeof(int32); + + p+=sizeof(int32); + + for(i=0;i<40;++i) (*way)->cmnt[i] = *p++; + + (*way)->smbl = *p++; + (*way)->dspl = *p++; + + (*way)->dst=GPS_Util_Get_Float(p); + + return; +} + + +/* @funcstatic GPS_D450_Get ******************************************** +** +** Get proximity waypoint data +** +** @param [w] way [GPS_PWay *] waypoint array +** @param [r] s [UC *] packet data +** +** @return [void] +************************************************************************/ +static void GPS_D450_Get(GPS_PWay *way, UC *s) +{ + UC *p; + int32 i; + + p=s; + + (*way)->prot = 450; + + (*way)->idx = GPS_Util_Get_Short(p); + p+=sizeof(int16); + + for(i=0;i<6;++i) (*way)->ident[i] = *p++; + for(i=0;i<2;++i) (*way)->cc[i] = *p++; + (*way)->wpt_class = *p++; + + (*way)->lat = GPS_Math_Semi_To_Deg(GPS_Util_Get_Int(p)); + p+=sizeof(int32); + + (*way)->lon = GPS_Math_Semi_To_Deg(GPS_Util_Get_Int(p)); + p+=sizeof(int32); + + (*way)->alt = GPS_Util_Get_Short(p); + p+=sizeof(int16); + + for(i=0;i<24;++i) (*way)->city[i] = *p++; + for(i=0;i<2;++i) (*way)->state[i] = *p++; + for(i=0;i<30;++i) (*way)->name[i] = *p++; + for(i=0;i<40;++i) (*way)->cmnt[i] = *p++; + + (*way)->dst=GPS_Util_Get_Float(p); + + return; +} + + +/* @funcstatic GPS_D400_Send ******************************************** +** +** Form proximity waypoint data string +** +** @param [w] data [UC *] string to write to +** @param [r] way [GPS_PWay] waypoint data +** @param [w] len [int32 *] packet length +** +** @return [void] +************************************************************************/ +static void GPS_D400_Send(UC *data, GPS_PWay way, int32 *len) +{ + UC *p; + int32 i; + + p = data; + + for(i=0;i<6;++i) *p++ = way->ident[i]; + GPS_Util_Put_Int(p,(int32)GPS_Math_Deg_To_Semi(way->lat)); + p+=sizeof(int32); + GPS_Util_Put_Int(p,(int32)GPS_Math_Deg_To_Semi(way->lon)); + p+=sizeof(int32); + GPS_Util_Put_Uint(p,0); + p+=sizeof(int32); + for(i=0;i<40;++i) *p++ = way->cmnt[i]; + + GPS_Util_Put_Float(p,way->dst); + + *len = 62; + + return; +} + + +/* @funcstatic GPS_D403_Send ******************************************* +** +** Form proximity waypoint data string +** +** @param [w] data [UC *] string to write to +** @param [r] way [GPS_PWay] waypoint data +** @param [w] len [int32 *] packet length +** +** @return [void] +************************************************************************/ +static void GPS_D403_Send(UC *data, GPS_PWay way, int32 *len) +{ + UC *p; + int32 i; + + p = data; + + for(i=0;i<6;++i) *p++ = way->ident[i]; + GPS_Util_Put_Int(p,(int32)GPS_Math_Deg_To_Semi(way->lat)); + p+=sizeof(int32); + GPS_Util_Put_Int(p,(int32)GPS_Math_Deg_To_Semi(way->lon)); + p+=sizeof(int32); + GPS_Util_Put_Uint(p,0); + p+=sizeof(int32); + for(i=0;i<40;++i) *p++ = way->cmnt[i]; + + *p++ = way->smbl; + *p = way->dspl; + + GPS_Util_Put_Float(p,way->dst); + + *len = 64; + + return; +} + + +/* @funcstatic GPS_D450_Send ******************************************* +** +** Form proximity waypoint data string +** +** @param [w] data [UC *] string to write to +** @param [r] way [GPS_PWay] waypoint data +** @param [w] len [int32 *] packet length +** +** @return [void] +************************************************************************/ +static void GPS_D450_Send(UC *data, GPS_PWay way, int32 *len) +{ + UC *p; + int32 i; + + p = data; + + GPS_Util_Put_Short(p,way->idx); + p+=sizeof(int16); + + for(i=0;i<6;++i) *p++ = way->ident[i]; + for(i=0;i<2;++i) *p++ = way->cc[i]; + *p++ = way->wpt_class; + + GPS_Util_Put_Int(p,(int32)GPS_Math_Deg_To_Semi(way->lat)); + p+=sizeof(int32); + GPS_Util_Put_Int(p,(int32)GPS_Math_Deg_To_Semi(way->lon)); + p+=sizeof(int32); + + GPS_Util_Put_Short(p,way->alt); + p+=sizeof(int16); + + for(i=0;i<24;++i) *p++ = way->city[i]; + for(i=0;i<2;++i) *p++ = way->state[i]; + for(i=0;i<30;++i) *p++ = way->name[i]; + for(i=0;i<40;++i) *p++ = way->cmnt[i]; + + GPS_Util_Put_Float(p,way->dst); + + + *len = 121; + + return; +} + + + +/* @func GPS_A500_Get ****************************************************** +** +** Get almanac from GPS +** +** @param [r] port [const char *] serial port +** @param [w] alm [GPS_PAlmanac **] almanac array +** +** @return [int32] number of almanac entries +************************************************************************/ +int32 GPS_A500_Get(const char *port, GPS_PAlmanac **alm) +{ + static UC data[2]; + int32 fd; + GPS_PPacket tra; + GPS_PPacket rec; + int32 n; + int32 i; + int32 ret; + + if(!GPS_Serial_On(port, &fd)) + return gps_errno; + + if(!(tra = GPS_Packet_New()) || !(rec = GPS_Packet_New())) + return MEMORY_ERROR; + + + GPS_Util_Put_Short(data, + COMMAND_ID[gps_device_command].Cmnd_Transfer_Alm); + GPS_Make_Packet(&tra, LINK_ID[gps_link_type].Pid_Command_Data, + data,2); + if(!GPS_Write_Packet(fd,tra)) + return gps_errno; + if(!GPS_Get_Ack(fd, &tra, &rec)) + return gps_errno; + + if(!GPS_Packet_Read(fd, &rec)) + return gps_errno; + if(!GPS_Send_Ack(fd, &tra, &rec)) + return gps_errno; + + n = GPS_Util_Get_Short(rec->data); + + if(n) + if(!((*alm)=(GPS_PAlmanac *)malloc(n*sizeof(GPS_PAlmanac)))) + { + GPS_Error("A500_Get: Insufficient memory"); + return MEMORY_ERROR; + } + for(i=0;itype == LINK_ID[gps_link_type].Pid_Command_Data && + GPS_Util_Get_Short(rec->data) == COMMAND_ID[gps_device_command]. + Cmnd_Transfer_Time) + { + GPS_User("INFO: GPS time request. Sending...."); + ret = GPS_Rqst_Send_Time(fd,gps_save_time); + if(ret < 0) return ret; + timesent=1; + } + } + + + + /* + * Allow GPS a little while to decide whether it wants to ask for + * the position. Note that the posn sent is held in gps_save_lat + * and gps_save_lon global! + */ + if(GPS_Serial_Wait(fd)) + { + if(!GPS_Packet_Read(fd, &rec)) + return gps_errno; + + if(!GPS_Send_Ack(fd, &tra, &rec)) + return gps_errno; + + if(rec->type == LINK_ID[gps_link_type].Pid_Command_Data && + GPS_Util_Get_Short(rec->data) == COMMAND_ID[gps_device_command]. + Cmnd_Transfer_Posn) + { + GPS_User("INFO: GPS position request. Sending...."); + ret = GPS_Rqst_Send_Position(fd,gps_save_lat,gps_save_lon); + if(ret < 0) return ret; + posnsent=1; + } + } + + if(!timesent) + { + ret = GPS_Rqst_Send_Time(fd,gps_save_time); + if(ret < 0) return ret; + } + + + if(!posnsent) + { + ret = GPS_Rqst_Send_Position(fd,gps_save_lat,gps_save_lon); + if(ret < 0) return ret; + } + + + GPS_Packet_Del(&tra); + GPS_Packet_Del(&rec); + + if(!GPS_Serial_Off(port, fd)) + return gps_errno; + + return 1; +} + + + +/* @funcstatic GPS_D500_Get ******************************************** +** +** Get almanac data +** +** @param [w] alm [GPS_PAlmanac *] almanac array +** @param [r] entries [int32] number of packets to receive +** @param [r] fd [int32] file descriptor +** +** @return [int32] number of entries read +************************************************************************/ +static int32 GPS_D500_Get(GPS_PAlmanac *alm, int32 entries, int32 fd) +{ + GPS_PPacket tra; + GPS_PPacket rec; + int32 i; + + if(!(tra = GPS_Packet_New()) || !(rec = GPS_Packet_New())) + return MEMORY_ERROR; + + + for(i=0;idata, &alm[i]); + } + + + if(!GPS_Packet_Read(fd, &rec)) + return gps_errno; + + if(!GPS_Send_Ack(fd, &tra, &rec)) + return gps_errno; + + + if(rec->type != LINK_ID[gps_link_type].Pid_Xfer_Cmplt) + { + GPS_Error("D500_GET: Error transferring almanac"); + return FRAMING_ERROR; + } + + GPS_Packet_Del(&tra); + GPS_Packet_Del(&rec); + + return i; +} + + +/* @funcstatic GPS_D501_Get ******************************************** +** +** Get almanac data +** +** @param [w] alm [GPS_PAlmanac *] almanac array +** @param [r] entries [int32] number of packets to receive +** @param [r] fd [int32] file descriptor +** +** @return [int32] number of entries read +************************************************************************/ +static int32 GPS_D501_Get(GPS_PAlmanac *alm, int32 entries, int32 fd) +{ + GPS_PPacket tra; + GPS_PPacket rec; + int32 i; + + if(!(tra = GPS_Packet_New()) || !(rec = GPS_Packet_New())) + return MEMORY_ERROR; + + + for(i=0;idata, &alm[i]); + alm[i]->hlth=rec->data[42]; + } + + + if(!GPS_Packet_Read(fd, &rec)) + return gps_errno; + if(!GPS_Send_Ack(fd, &tra, &rec)) + return gps_errno; + + + if(rec->type != LINK_ID[gps_link_type].Pid_Xfer_Cmplt) + { + GPS_Error("D501_GET: Error transferring almanac"); + return FRAMING_ERROR; + } + + GPS_Packet_Del(&tra); + GPS_Packet_Del(&rec); + + return i; +} + + + +/* @funcstatic GPS_D550_Get ********************************************* +** +** Get almanac data +** +** @param [w] alm [GPS_PAlmanac *] almanac array +** @param [r] entries [int32] number of packets to receive +** @param [r] fd [int32] file descriptor +** +** @return [int32] number of entries read +************************************************************************/ +static int32 GPS_D550_Get(GPS_PAlmanac *alm, int32 entries, int32 fd) +{ + GPS_PPacket tra; + GPS_PPacket rec; + int32 i; + + if(!(tra = GPS_Packet_New()) || !(rec = GPS_Packet_New())) + return MEMORY_ERROR; + + + for(i=0;isvid = rec->data[0]; + GPS_A500_Translate(rec->data+1, &alm[i]); + } + + + if(!GPS_Packet_Read(fd, &rec)) + return gps_errno; + if(!GPS_Send_Ack(fd, &tra, &rec)) + return gps_errno; + + if(rec->type != LINK_ID[gps_link_type].Pid_Xfer_Cmplt) + { + GPS_Error("D550_GET: Error transferring almanac"); + return FRAMING_ERROR; + } + + GPS_Packet_Del(&tra); + GPS_Packet_Del(&rec); + + return i; +} + + + +/* @funcstatic GPS_D551_Get ********************************************* +** +** Get almanac data +** +** @param [w] alm [GPS_PAlmanac *] almanac array +** @param [r] entries [int32] number of packets to receive +** @param [r] fd [int32] file descriptor +** +** @return [int32] number of entries read +************************************************************************/ +static int32 GPS_D551_Get(GPS_PAlmanac *alm, int32 entries, int32 fd) +{ + GPS_PPacket tra; + GPS_PPacket rec; + int32 i; + + if(!(tra = GPS_Packet_New()) || !(rec = GPS_Packet_New())) + return MEMORY_ERROR; + + + for(i=0;isvid = rec->data[0]; + GPS_A500_Translate(rec->data+1, &alm[i]); + alm[i]->hlth = rec->data[43]; + } + + + if(!GPS_Packet_Read(fd, &rec)) + return gps_errno; + if(!GPS_Send_Ack(fd, &tra, &rec)) + return gps_errno; + + + if(rec->type != LINK_ID[gps_link_type].Pid_Xfer_Cmplt) + { + GPS_Error("D551_GET: Error transferring almanac\n"); + return FRAMING_ERROR; + } + + GPS_Packet_Del(&tra); + GPS_Packet_Del(&rec); + + return i; +} + + + +/* @funcstatic GPS_A500_Translate *************************************** +** +** Translate almanac packet to almanac structure +** +** @param [r] s [const UC *] almanac packet data +** @param [w] alm [GPS_PAlmanac *] almanac entry pointer +** +** @return [void] +************************************************************************/ +static void GPS_A500_Translate(UC *s, GPS_PAlmanac *alm) +{ + UC *p; + + p=s; + + (*alm)->wn = GPS_Util_Get_Short(p); + p+=sizeof(int16); + + (*alm)->toa = GPS_Util_Get_Float(p); + p+=sizeof(float); + + (*alm)->af0 = GPS_Util_Get_Float(p); + p+=sizeof(float); + + (*alm)->af1 = GPS_Util_Get_Float(p); + p+=sizeof(float); + + (*alm)->e = GPS_Util_Get_Float(p); + p+=sizeof(float); + + (*alm)->sqrta = GPS_Util_Get_Float(p); + p+=sizeof(float); + + (*alm)->m0 = GPS_Util_Get_Float(p); + p+=sizeof(float); + + (*alm)->w = GPS_Util_Get_Float(p); + p+=sizeof(float); + + (*alm)->omg0 = GPS_Util_Get_Float(p); + p+=sizeof(float); + + (*alm)->odot = GPS_Util_Get_Float(p); + p+=sizeof(float); + + (*alm)->i = GPS_Util_Get_Float(p); + p+=sizeof(float); + + return; +} + + +/* @funcstatic GPS_D500_Send ******************************************* +** +** Form almanac data string +** +** @param [w] data [UC *] string to write to +** @param [r] alm [GPS_PAlmanac] almanac data +** +** @return [void] +************************************************************************/ +static void GPS_D500_Send(UC *data, GPS_PAlmanac alm) +{ + UC *p; + + p = data; + GPS_A500_Encode(p,alm); + + return; +} + + + +/* @funcstatic GPS_D501_Send ******************************************** +** +** Form almanac data string +** +** @param [w] data [UC *] string to write to +** @param [r] alm [GPS_PAlmanac] almanac data +** +** @return [void] +************************************************************************/ +static void GPS_D501_Send(UC *data, GPS_PAlmanac alm) +{ + UC *p; + + p=data; + p[42] = alm->hlth; + GPS_A500_Encode(p,alm); + + return; +} + + + +/* @funcstatic GPS_D550_Send ******************************************** +** +** Form almanac data string +** +** @param [w] data [UC *] string to write to +** @param [r] alm [GPS_PAlmanac] almanac data +** +** @return [void] +************************************************************************/ +static void GPS_D550_Send(UC *data, GPS_PAlmanac alm) +{ + UC *p; + + p = data; + *p = alm->svid; + GPS_A500_Encode(p+1,alm); + + return; +} + + + +/* @funcstatic GPS_D551_Send ******************************************** +** +** Form almanac data string +** +** @param [w] data [UC *] string to write to +** @param [r] alm [GPS_PAlmanac] almanac data +** +** @return [void] +************************************************************************/ +static void GPS_D551_Send(UC *data, GPS_PAlmanac alm) +{ + UC *p; + + p = data; + *p = alm->svid; + GPS_A500_Encode(p+1,alm); + p[43] = alm->hlth; + + return; +} + + + +/* @funcstatic GPS_A500_Encode *************************************** +** +** Encode almanac structure to almanac packet +** +** @param [w] s [UC *] string to write to +** @param [r] alm [GPS_PAlmanac] almanac entry +** +** @return [void] +************************************************************************/ +static void GPS_A500_Encode(UC *s, GPS_PAlmanac alm) +{ + UC *p; + + p=s; + + GPS_Util_Put_Short(p,alm->wn); + p+=sizeof(int16); + + GPS_Util_Put_Float(p,alm->toa); + p+=sizeof(float); + + GPS_Util_Put_Float(p,alm->af0); + p+=sizeof(float); + + GPS_Util_Put_Float(p,alm->af1); + p+=sizeof(float); + + GPS_Util_Put_Float(p,alm->e); + p+=sizeof(float); + + GPS_Util_Put_Float(p,alm->sqrta); + p+=sizeof(float); + + GPS_Util_Put_Float(p,alm->m0); + p+=sizeof(float); + + GPS_Util_Put_Float(p,alm->w); + p+=sizeof(float); + + GPS_Util_Put_Float(p,alm->omg0); + p+=sizeof(float); + + GPS_Util_Put_Float(p,alm->odot); + p+=sizeof(float); + + GPS_Util_Put_Float(p,alm->i); + + return; +} + + +/* @func GPS_A600_Get ****************************************************** +** +** Get time from GPS +** +** @param [r] port [const char *] serial port +** +** @return [time_t] GPS time as unix system time, -ve if error +************************************************************************/ +time_t GPS_A600_Get(const char *port) +{ + static UC data[2]; + int32 fd; + GPS_PPacket tra; + GPS_PPacket rec; + time_t ret; + + if(!GPS_Serial_On(port, &fd)) + return gps_errno; + + if(!(tra = GPS_Packet_New()) || !(rec = GPS_Packet_New())) + return MEMORY_ERROR; + + + GPS_Util_Put_Short(data, + COMMAND_ID[gps_device_command].Cmnd_Transfer_Time); + GPS_Make_Packet(&tra, LINK_ID[gps_link_type].Pid_Command_Data, + data,2); + if(!GPS_Write_Packet(fd,tra)) + return gps_errno; + if(!GPS_Get_Ack(fd, &tra, &rec)) + return gps_errno; + + if(!GPS_Packet_Read(fd, &rec)) + return gps_errno; + if(!GPS_Send_Ack(fd, &tra, &rec)) + return gps_errno; + + switch(gps_date_time_type) + { + case pD600: + ret = GPS_D600_Get(rec); + break; + default: + GPS_Error("A600_Get: Unknown data/time protocol"); + return PROTOCOL_ERROR; + } + + GPS_Packet_Del(&tra); + GPS_Packet_Del(&rec); + + if(!GPS_Serial_Off(port, fd)) + return gps_errno; + + return ret; +} + + + + + +/* @func GPS_A600_Send ************************************************** +** +** Send time to GPS +** +** @param [r] port [const char *] serial port +** @param [r] Time [time_t] unix-style time +** +** @return [int32] success +************************************************************************/ +int32 GPS_A600_Send(const char *port, time_t Time) +{ + int32 fd; + GPS_PPacket tra; + GPS_PPacket rec; + int32 posnsent=0; + int32 ret=0; + + if(!GPS_Serial_On(port, &fd)) + return gps_errno; + + if(!(tra = GPS_Packet_New()) || !(rec = GPS_Packet_New())) + return MEMORY_ERROR; + + + switch(gps_date_time_type) + { + case pD600: + GPS_D600_Send(&tra,Time); + break; + default: + GPS_Error("A600_Send: Unknown data/time protocol"); + return PROTOCOL_ERROR; + } + + if(!GPS_Write_Packet(fd,tra)) + return gps_error; + if(!GPS_Get_Ack(fd, &tra, &rec)) + return gps_error; + + + /* + * Allow GPS a little while to decide whether it wants to ask for + * the position. Note that the posn sent is held in gps_save_lat + * and gps_save_lon globals! + */ + if(GPS_Serial_Wait(fd)) + { + if(!GPS_Packet_Read(fd, &rec)) + return gps_errno; + + if(!GPS_Send_Ack(fd, &tra, &rec)) + return gps_errno; + + if(rec->type == LINK_ID[gps_link_type].Pid_Command_Data && + GPS_Util_Get_Short(rec->data) == COMMAND_ID[gps_device_command]. + Cmnd_Transfer_Posn) + { + GPS_User("INFO: GPS position request. Sending...."); + ret = GPS_Rqst_Send_Position(fd,gps_save_lat,gps_save_lon); + if(ret < 0) return ret; + posnsent=1; + } + } + + + if(!posnsent) + { + ret = GPS_Rqst_Send_Position(fd,gps_save_lat,gps_save_lon); + if(ret < 0) return ret; + } + + + GPS_Packet_Del(&tra); + GPS_Packet_Del(&rec); + + if(!GPS_Serial_Off(port, fd)) + return gps_errno; + + return 1; +} + + + + + +/* @func GPS_D600_Get ****************************************************** +** +** Convert date/time packet to ints +** +** @param [r] packet [GPS_PPacket] packet +** +** @return [time_t] gps time as unix system time +************************************************************************/ +time_t GPS_D600_Get(GPS_PPacket packet) +{ + UC *p; + static struct tm ts; + + p = packet->data; + + ts.tm_mon = *p++ - 1; + ts.tm_mday = *p++; + ts.tm_year = (int32) GPS_Util_Get_Short(p) - 1900; + p+=2; + ts.tm_hour = (int32) GPS_Util_Get_Short(p); + p+=2; + ts.tm_min = *p++; + ts.tm_sec = *p++; + + return mktime(&ts); +} + + +/* @func GPS_D600_Send ****************************************************** +** +** make a time packet for sending to the GPS +** +** @param [w] packet [GPS_PPacket *] packet +** @param [r] Time [time_t] unix-style time +** +** @return [void] +************************************************************************/ +void GPS_D600_Send(GPS_PPacket *packet, time_t Time) +{ + UC data[10]; + UC *p; + struct tm *ts; + + p = data; + + ts = localtime(&Time); + *p++ = ts->tm_mon+1; + *p++ = ts->tm_mday; + + GPS_Util_Put_Short(p,ts->tm_year+1900); + p+=2; + GPS_Util_Put_Short(p,ts->tm_hour); + p+=2; + + *p++ = ts->tm_min; + *p = ts->tm_sec; + + GPS_Make_Packet(packet, LINK_ID[gps_link_type].Pid_Date_Time_Data, + data,8); + + return; +} + + + + +/* @func GPS_A700_Get ****************************************************** +** +** Get position from GPS +** +** @param [r] port [const char *] serial port +** @param [w] lat [double *] latitude (deg) +** @param [w] lon [double *] longitude (deg) +** +** @return [int32] success +************************************************************************/ +int32 GPS_A700_Get(const char *port, double *lat, double *lon) +{ + static UC data[2]; + int32 fd; + GPS_PPacket tra; + GPS_PPacket rec; + + if(!GPS_Serial_On(port, &fd)) + return gps_errno; + + if(!(tra = GPS_Packet_New()) || !(rec = GPS_Packet_New())) + return MEMORY_ERROR; + + + GPS_Util_Put_Short(data, + COMMAND_ID[gps_device_command].Cmnd_Transfer_Posn); + GPS_Make_Packet(&tra, LINK_ID[gps_link_type].Pid_Command_Data, + data,2); + if(!GPS_Write_Packet(fd,tra)) + return gps_errno; + if(!GPS_Get_Ack(fd, &tra, &rec)) + return gps_errno; + + if(!GPS_Packet_Read(fd, &rec)) + return gps_errno; + if(!GPS_Send_Ack(fd, &tra, &rec)) + return gps_errno; + + switch(gps_position_type) + { + case pD700: + GPS_D700_Get(rec, lat, lon); + break; + default: + GPS_Error("A700_Get: Unknown position protocol"); + return PROTOCOL_ERROR; + } + + GPS_Packet_Del(&tra); + GPS_Packet_Del(&rec); + + if(!GPS_Serial_Off(port, fd)) + return gps_errno; + + return 1; +} + + + +/* @func GPS_A700_Send ****************************************************** +** +** Send position to GPS +** +** @param [r] port [const char *] serial port +** @param [r] lat [double] latitude (deg) +** @param [r] lon [double] longitute (deg) +** +** @return [int32] success +************************************************************************/ +int32 GPS_A700_Send(const char *port, double lat, double lon) +{ + int32 fd; + GPS_PPacket tra; + GPS_PPacket rec; + + if(!GPS_Serial_On(port, &fd)) + return gps_errno; + + if(!(tra = GPS_Packet_New()) || !(rec = GPS_Packet_New())) + return MEMORY_ERROR; + + + switch(gps_position_type) + { + case pD700: + GPS_D700_Send(&tra,lat,lon); + break; + default: + GPS_Error("A700_Send: Unknown position protocol"); + return PROTOCOL_ERROR; + } + + if(!GPS_Write_Packet(fd,tra)) + return 0; + if(!GPS_Get_Ack(fd, &tra, &rec)) + return 0; + + + GPS_Packet_Del(&tra); + GPS_Packet_Del(&rec); + + if(!GPS_Serial_Off(port, fd)) + return gps_errno; + + return 1; +} + + + +/* @func GPS_D700_Get ****************************************************** +** +** Convert position packet to lat/long in degrees +** +** @param [r] packet [GPS_PPacket] packet +** @param [w] lat [double *] latitude (deg) +** @param [w] lon [double *] longitude (deg) +** +** @return [void] +************************************************************************/ +void GPS_D700_Get(GPS_PPacket packet, double *lat, double *lon) +{ + UC *p; + double t; + + p = packet->data; + + t = GPS_Util_Get_Double(p); + *lat = GPS_Math_Rad_To_Deg(t); + + p += sizeof(double); + + t = GPS_Util_Get_Double(p); + *lon = GPS_Math_Rad_To_Deg(t); + + + return; +} + + +/* @func GPS_D700_Send ****************************************************** +** +** make a position packet for sending to the GPS +** +** @param [w] packet [GPS_PPacket *] packet +** @param [r] lat [double] latitude (deg) +** @param [r] lon [double] longitude (deg) +** +** @return [void] +************************************************************************/ +void GPS_D700_Send(GPS_PPacket *packet, double lat, double lon) +{ + UC data[16]; + UC *p; + + lat = GPS_Math_Deg_To_Rad(lat); + lon = GPS_Math_Deg_To_Rad(lon); + + p = data; + + GPS_Util_Put_Double(p,lat); + p+=sizeof(double); + GPS_Util_Put_Double(p,lon); + + GPS_Make_Packet(packet, LINK_ID[gps_link_type].Pid_Position_Data, + data,16); + + return; +} + + + +/* @func GPS_A800_On ****************************************************** +** +** Turn on GPS PVT +** +** @param [r] port [const char *] serial port +** @param [w] fd [int32 *] file descriptor +** +** @return [int32] success +************************************************************************/ +int32 GPS_A800_On(const char *port, int32 *fd) +{ + static UC data[2]; + GPS_PPacket tra; + GPS_PPacket rec; + + if(!GPS_Serial_On(port, fd)) + return gps_errno; + + if(!(tra = GPS_Packet_New()) || !(rec = GPS_Packet_New())) + return MEMORY_ERROR; + + + GPS_Util_Put_Short(data, + COMMAND_ID[gps_device_command].Cmnd_Start_Pvt_Data); + GPS_Make_Packet(&tra, LINK_ID[gps_link_type].Pid_Command_Data, + data,2); + if(!GPS_Write_Packet(*fd,tra)) + return gps_errno; + if(!GPS_Get_Ack(*fd, &tra, &rec)) + { + GPS_Error("A800_on: Pvt start data not acknowledged"); + return FRAMING_ERROR; + } + + GPS_Packet_Del(&rec); + GPS_Packet_Del(&tra); + + return 1; +} + + + +/* @func GPS_A800_Off ****************************************************** +** +** Turn off GPS PVT +** +** @param [r] port [const char *] port +** @param [w] fd [int32 *] file descriptor +** +** @return [int32] success +************************************************************************/ +int32 GPS_A800_Off(const char *port, int32 *fd) +{ + static UC data[2]; + GPS_PPacket tra; + GPS_PPacket rec; + + if(!(tra = GPS_Packet_New()) || !(rec = GPS_Packet_New())) + return MEMORY_ERROR; + + + GPS_Util_Put_Short(data, + COMMAND_ID[gps_device_command].Cmnd_Stop_Pvt_Data); + GPS_Make_Packet(&tra, LINK_ID[gps_link_type].Pid_Command_Data, + data,2); + if(!GPS_Write_Packet(*fd,tra)) + return gps_errno; + if(!GPS_Get_Ack(*fd, &tra, &rec)) + { + GPS_Error("A800_Off: Not acknowledged"); + return FRAMING_ERROR; + } + + + GPS_Packet_Del(&rec); + GPS_Packet_Del(&tra); + + if(!GPS_Serial_Off(port, *fd)) + return gps_errno; + + return 1; +} + + +/* @func GPS_A800_Get ************************************************** +** +** make a position packet for sending to the GPS +** +** @param [r] fd [int32 *] file descriptor +** @param [w] packet [GPS_PPvt_Data *] packet +** +** @return [int32] success +************************************************************************/ +int32 GPS_A800_Get(int32 *fd, GPS_PPvt_Data *packet) +{ + GPS_PPacket tra; + GPS_PPacket rec; + + + if(!(tra = GPS_Packet_New()) || !(rec = GPS_Packet_New())) + return MEMORY_ERROR; + + + if(!GPS_Packet_Read(*fd, &rec)) + return gps_errno; + + if(!GPS_Send_Ack(*fd, &tra, &rec)) + return gps_errno; + + switch(gps_pvt_type) + { + case pD800: + GPS_D800_Get(rec,packet); + break; + default: + GPS_Error("A800_GET: Unknown pvt protocol"); + return PROTOCOL_ERROR; + } + + GPS_Packet_Del(&rec); + GPS_Packet_Del(&tra); + + return 1; +} + + + +/* @func GPS_D800_Get ****************************************************** +** +** Convert packet to pvt structure +** +** @param [r] packet [GPS_PPacket] packet +** @param [w] pvt [GPS_PPvt_Data *] pvt structure +** +** @return [void] +************************************************************************/ +void GPS_D800_Get(GPS_PPacket packet, GPS_PPvt_Data *pvt) +{ + UC *p; + + p = packet->data; + + (*pvt)->alt = GPS_Util_Get_Float(p); + p+=sizeof(float); + + (*pvt)->epe = GPS_Util_Get_Float(p); + p+=sizeof(float); + + (*pvt)->eph = GPS_Util_Get_Float(p); + p+=sizeof(float); + + (*pvt)->epv = GPS_Util_Get_Float(p); + p+=sizeof(float); + + (*pvt)->fix = GPS_Util_Get_Short(p); + p+=sizeof(int16); + + (*pvt)->tow = GPS_Util_Get_Double(p); + p+=sizeof(double); + + (*pvt)->lat = GPS_Math_Rad_To_Deg(GPS_Util_Get_Double(p)); + p+=sizeof(double); + + (*pvt)->lon = GPS_Math_Rad_To_Deg(GPS_Util_Get_Double(p)); + p+=sizeof(double); + + (*pvt)->east = GPS_Util_Get_Float(p); + p+=sizeof(float); + + (*pvt)->north = GPS_Util_Get_Float(p); + p+=sizeof(float); + + (*pvt)->up = GPS_Util_Get_Float(p); + p+=sizeof(float); + + (*pvt)->msl_hght = GPS_Util_Get_Float(p); + p+=sizeof(float); + + (*pvt)->leap_scnds = GPS_Util_Get_Short(p); + p+=sizeof(int16); + + (*pvt)->wn_days = GPS_Util_Get_Int(p); + + return; +} + + + diff --git a/gpsbabel/jeeps/gpsapp.h b/gpsbabel/jeeps/gpsapp.h new file mode 100644 index 000000000..118895303 --- /dev/null +++ b/gpsbabel/jeeps/gpsapp.h @@ -0,0 +1,62 @@ +#ifdef __cplusplus +extern "C" +{ +#endif + +#ifndef gpsapp_h +#define gpsapp_h + + +#include "gps.h" + +int32 GPS_Init(const char *port); + +int32 GPS_A100_Get(const char *port, GPS_PWay **way); +int32 GPS_A100_Send(const char *port, GPS_PWay *way, int32 n); + +int32 GPS_A200_Get(const char *port, GPS_PWay **way); +int32 GPS_A201_Get(const char *port, GPS_PWay **way); +int32 GPS_A200_Send(const char *port, GPS_PWay *way, int32 n); +int32 GPS_A201_Send(const char *port, GPS_PWay *way, int32 n); + +int32 GPS_A300_Get(const char *port, GPS_PTrack **trk); +int32 GPS_A301_Get(const char *port, GPS_PTrack **trk); +int32 GPS_A300_Send(const char *port, GPS_PTrack *trk, int32 n); +int32 GPS_A301_Send(const char *port, GPS_PTrack *trk, int32 n); + +int32 GPS_D300_Get(GPS_PTrack *trk, int32 entries, int32 fd); +void GPS_D300b_Get(GPS_PTrack *trk, UC *data); +void GPS_D301b_Get(GPS_PTrack *trk, UC *data); +void GPS_D310_Get(GPS_PTrack *trk, UC *s); +void GPS_D300_Send(UC *data, GPS_PTrack trk); +void GPS_D301_Send(UC *data, GPS_PTrack trk); +void GPS_D310_Send(UC *data, GPS_PTrack trk, int32 *len); + +int32 GPS_A400_Get(const char *port, GPS_PWay **way); +int32 GPS_A400_Send(const char *port, GPS_PWay *way, int32 n); + +int32 GPS_A500_Get(const char *port, GPS_PAlmanac **alm); +int32 GPS_A500_Send(const char *port, GPS_PAlmanac *alm, int32 n); + +time_t GPS_A600_Get(const char *port); +time_t GPS_D600_Get(GPS_PPacket packet); +int32 GPS_A600_Send(const char *port, time_t Time); +void GPS_D600_Send(GPS_PPacket *packet, time_t Time); + +int32 GPS_A700_Get(const char *port, double *lat, double *lon); +int32 GPS_A700_Send(const char *port, double lat, double lon); +void GPS_D700_Get(GPS_PPacket packet, double *lat, double *lon); +void GPS_D700_Send(GPS_PPacket *packet, double lat, double lon); + +int32 GPS_A800_On(const char *port, int32 *fd); +int32 GPS_A800_Off(const char *port, int32 *fd); +int32 GPS_A800_Get(int32 *fd, GPS_PPvt_Data *packet); +void GPS_D800_Get(GPS_PPacket packet, GPS_PPvt_Data *pvt); + + + +#endif + +#ifdef __cplusplus +} +#endif diff --git a/gpsbabel/jeeps/gpscom.c b/gpsbabel/jeeps/gpscom.c new file mode 100644 index 000000000..d1d07d704 --- /dev/null +++ b/gpsbabel/jeeps/gpscom.c @@ -0,0 +1,606 @@ +/******************************************************************** +** @source JEEPS command functions +** +** @author Copyright (C) 1999 Alan Bleasby +** @version 1.0 +** @modified Dec 28 1999 Alan Bleasby. First version +** @@ +** +** This library is free software; you can redistribute it and/or +** modify it under the terms of the GNU Library General Public +** License as published by the Free Software Foundation; either +** version 2 of the License, or (at your option) any later version. +** +** This library is distributed in the hope that it will be useful, +** but WITHOUT ANY WARRANTY; without even the implied warranty of +** MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU +** Library General Public License for more details. +** +** You should have received a copy of the GNU Library General Public +** License along with this library; if not, write to the +** Free Software Foundation, Inc., 59 Temple Place - Suite 330, +** Boston, MA 02111-1307, USA. +********************************************************************/ +#include "gps.h" +#include + + +/* @func GPS_Command_Off *********************************************** +** +** Turn off power on GPS +** +** @return [int32] success +************************************************************************/ + +int32 GPS_Command_Off(const char *port) +{ + static UC data[2]; + int32 fd; + GPS_PPacket tra; + GPS_PPacket rec; + + if(!GPS_Serial_On(port, &fd)) + return gps_errno; + + if(!(tra = GPS_Packet_New()) || !(rec = GPS_Packet_New())) + return MEMORY_ERROR; + + GPS_Util_Put_Short(data,COMMAND_ID[gps_device_command].Cmnd_Turn_Off_Pwr); + + GPS_Make_Packet(&tra, LINK_ID[gps_link_type].Pid_Command_Data, + data,2); + if(!GPS_Write_Packet(fd,tra)) + return gps_errno; + + if(!GPS_Serial_Chars_Ready(fd)) + { + if(!GPS_Get_Ack(fd, &tra, &rec)) + return gps_errno; + GPS_User("Power off command acknowledged"); + } + + GPS_Packet_Del(&tra); + GPS_Packet_Del(&rec); + + if(!GPS_Serial_Off(port, fd)) + return gps_errno; + + return 1; +} + + +/* @func GPS_Command_Get_Waypoint *************************************** +** +** Get waypoint from GPS +** +** @param [r] port [const char *] serial port +** @param [w] way [GPS_PWay **] pointer to waypoint array +** +** @return [int32] number of waypoint entries +************************************************************************/ + +int32 GPS_Command_Get_Waypoint(const char *port, GPS_PWay **way) +{ + int32 ret=0; + + switch(gps_waypt_transfer) + { + case pA100: + ret = GPS_A100_Get(port,way); + break; + default: + GPS_Error("Get_Waypoint: Unknown waypoint protocol"); + return PROTOCOL_ERROR; + } + + return ret; +} + + + +/* @func GPS_Command_Send_Waypoint ****************************************** +** +** Send waypoints to GPS +** +** @param [r] port [const char *] serial port +** @param [r] way [GPS_PWay *] waypoint array +** @param [r] n [int32] number of waypoint entries +** +** @return [int32] success +************************************************************************/ + +int32 GPS_Command_Send_Waypoint(const char *port, GPS_PWay *way, int32 n) +{ + int32 ret=0; + + switch(gps_waypt_transfer) + { + case pA100: + ret = GPS_A100_Send(port, way, n); + break; + default: + GPS_Error("Send_Waypoint: Unknown waypoint protocol"); + return PROTOCOL_ERROR; + } + + return ret; +} + + +/* @func GPS_Command_Get_Route ************************************** +** +** Get Route(s) from GPS +** +** @param [r] port [const char *] serial port +** @param [w] way [GPS_PWay **] pointer to waypoint array +** +** @return [int32] number of waypoint entries +************************************************************************/ + +int32 GPS_Command_Get_Route(const char *port, GPS_PWay **way) +{ + int32 ret=0; + + switch(gps_route_transfer) + { + case pA200: + ret = GPS_A200_Get(port,way); + break; + case pA201: + ret = GPS_A201_Get(port,way); + break; + default: + GPS_Error("Get_Route: Unknown route protocol"); + return PROTOCOL_ERROR; + } + + return ret; +} + + + +/* @func GPS_Command_Send_Route **************************************** +** +** Send route(s) to GPS +** +** @param [r] port [const char *] serial port +** @param [r] way [GPS_PWay *] waypoint array +** @param [r] n [int32] number of waypoint entries +** +** @return [int32] success +************************************************************************/ + +int32 GPS_Command_Send_Route(const char *port, GPS_PWay *way, int32 n) +{ + int32 ret=0; + + + switch(gps_route_transfer) + { + case pA200: + ret = GPS_A200_Send(port, way, n); + break; + case pA201: + ret = GPS_A201_Send(port, way, n); + break; + default: + GPS_Error("Send_Route: Unknown route protocol"); + return PROTOCOL_ERROR; + } + + return ret; +} + + +/* @func GPS_Command_Get_Track *************************************** +** +** Get track log from GPS +** +** @param [r] port [const char *] serial port +** @param [w] trk [GPS_PTrack **] pointer to track array +** +** @return [int32] number of track entries +************************************************************************/ + +int32 GPS_Command_Get_Track(const char *port, GPS_PTrack **trk) +{ + int32 ret=0; + + if(gps_trk_transfer == -1) + return GPS_UNSUPPORTED; + + switch(gps_trk_transfer) + { + case pA300: + ret = GPS_A300_Get(port,trk); + break; + case pA301: + ret = GPS_A301_Get(port,trk); + break; + default: + GPS_Error("Get_Track: Unknown track protocol\n"); + return PROTOCOL_ERROR; + } + + return ret; +} + + + +/* @func GPS_Command_Send_Track ****************************************** +** +** Send track log to GPS +** +** @param [r] port [const char *] serial port +** @param [r] trk [GPS_PTrack *] track array +** @param [r] n [int32] number of track entries +** +** @return [int32] success +************************************************************************/ + +int32 GPS_Command_Send_Track(const char *port, GPS_PTrack *trk, int32 n) +{ + int32 ret=0; + + if(gps_trk_transfer == -1) + return GPS_UNSUPPORTED; + + switch(gps_trk_transfer) + { + case pA300: + ret = GPS_A300_Send(port, trk, n); + break; + case pA301: + ret = GPS_A301_Send(port, trk, n); + break; + default: + GPS_Error("Send_Track: Unknown track protocol"); + break; + } + + return ret; +} + + +/* @func GPS_Command_Get_Proximity ************************************** +** +** Get proximitywaypoint from GPS +** +** @param [r] port [const char *] serial port +** @param [w] way [GPS_PWay **] pointer to waypoint array +** +** @return [int32] number of waypoint entries +************************************************************************/ + +int32 GPS_Command_Get_Proximity(const char *port, GPS_PWay **way) +{ + int32 ret=0; + + if(gps_prx_waypt_transfer == -1) + return GPS_UNSUPPORTED; + + switch(gps_prx_waypt_transfer) + { + case pA400: + ret = GPS_A400_Get(port,way); + break; + default: + GPS_Error("Get_Proximity: Unknown proximity protocol"); + return PROTOCOL_ERROR; + } + + return ret; +} + + + +/* @func GPS_Command_Send_Proximity ****************************************** +** +** Send proximity waypoints to GPS +** +** @param [r] port [const char *] serial port +** @param [r] way [GPS_PWay *] waypoint array +** @param [r] n [int32] number of waypoint entries +** +** @return [int32] success +************************************************************************/ + +int32 GPS_Command_Send_Proximity(const char *port, GPS_PWay *way, int32 n) +{ + int32 ret=0; + + + if(gps_prx_waypt_transfer == -1) + return GPS_UNSUPPORTED; + + + switch(gps_prx_waypt_transfer) + { + case pA400: + ret = GPS_A400_Send(port, way, n); + break; + default: + GPS_Error("Send_Proximity: Unknown proximity protocol"); + break; + } + + return ret; +} + + + +/* @func GPS_Command_Get_Almanac *************************************** +** +** Get almanac from GPS +** +** @param [r] port [const char *] serial port +** @param [w] alm [GPS_PAlmanac **] pointer to almanac array +** +** @return [int32] number of almanac entries +************************************************************************/ + +int32 GPS_Command_Get_Almanac(const char *port, GPS_PAlmanac **alm) +{ + int32 ret=0; + + switch(gps_almanac_transfer) + { + case pA500: + ret = GPS_A500_Get(port,alm); + break; + default: + GPS_Error("Get_Almanac: Unknown almanac protocol"); + return PROTOCOL_ERROR; + } + + return ret; +} + + + +/* @func GPS_Command_Send_Almanac ****************************************** +** +** Send almanac to GPS +** +** @param [r] port [const char *] serial port +** @param [r] alm [GPS_PAlmanac *] almanac array +** @param [r] n [int32] number of almanac entries +** +** @return [int32] success +************************************************************************/ + +int32 GPS_Command_Send_Almanac(const char *port, GPS_PAlmanac *alm, int32 n) +{ + int32 ret=0; + + switch(gps_almanac_transfer) + { + case pA500: + ret = GPS_A500_Send(port, alm, n); + break; + default: + GPS_Error("Send_Almanac: Unknown almanac protocol"); + break; + } + + return ret; +} + + + +/* @func GPS_Command_Get_Time ****************************************** +** +** Get time from GPS +** +** @param [r] port [const char *] serial port +** +** @return [time_t] unix-style time +************************************************************************/ + +time_t GPS_Command_Get_Time(const char *port) +{ + time_t ret=0; + + switch(gps_date_time_transfer) + { + case pA600: + ret = GPS_A600_Get(port); + break; + default: + GPS_Error("Get_Time: Unknown date/time protocol"); + return PROTOCOL_ERROR; + } + + return ret; +} + + + +/* @func GPS_Command_Send_Time ****************************************** +** +** Set GPS time +** +** @param [r] port [const char *] serial port +** @param [r] Time [time_t] unix-style time +** +** @return [int32] true if OK +************************************************************************/ + +int32 GPS_Command_Send_Time(const char *port, time_t Time) +{ + time_t ret=0; + + switch(gps_date_time_transfer) + { + case pA600: + ret = GPS_A600_Send(port, Time); + break; + default: + GPS_Error("Send_Time: Unknown date/time protocol"); + return PROTOCOL_ERROR; + } + + return ret; +} + + + + +/* @func GPS_Command_Get_Position *************************************** +** +** Get position from GPS +** +** @param [r] port [const char *] serial port +** @param [w] lat [double *] latitude (deg) +** @param [w] lon [double *] longitude (deg) +** +** @return [int32] success +************************************************************************/ + +int32 GPS_Command_Get_Position(const char *port, double *lat, double *lon) +{ + int32 ret=0; + + switch(gps_position_transfer) + { + case pA700: + ret = GPS_A700_Get(port,lat,lon); + break; + default: + GPS_Error("Get_Position: Unknown position protocol"); + return PROTOCOL_ERROR; + } + + return ret; +} + + + +/* @func GPS_Command_Send_Position ****************************************** +** +** Set GPS position +** +** @param [r] port [const char *] serial port +** @param [r] lat [double] latitude (deg) +** @param [r] lon [double] longitude (deg) +** +** @return [int32] success +************************************************************************/ + +int32 GPS_Command_Send_Position(const char *port, double lat, double lon) +{ + int32 ret=0; + + switch(gps_position_transfer) + { + case pA700: + ret = GPS_A700_Send(port, lat, lon); + break; + default: + GPS_Error("Send_Position: Unknown position protocol"); + return PROTOCOL_ERROR; + } + + return ret; +} + + +/* @func GPS_Command_Pvt_On ******************************************** +** +** Instruct GPS to start sending Pvt data every second +** +** @param [r] port [const char *] serial port +** @param [w] fd [int32 *] file descriptor +** +** @return [int32] success if supported and GPS starts sending +************************************************************************/ + +int32 GPS_Command_Pvt_On(const char *port, int32 *fd) +{ + int32 ret=0; + + + if(gps_pvt_transfer == -1) + return GPS_UNSUPPORTED; + + + switch(gps_pvt_transfer) + { + case pA800: + ret = GPS_A800_On(port,fd); + break; + default: + GPS_Error("Pvt_On: Unknown position protocol"); + return PROTOCOL_ERROR; + } + + + return ret; +} + + + +/* @func GPS_Command_Pvt_Off ******************************************** +** +** Instruct GPS to stop sending Pvt data every second +** +** @param [r] port [const char *] serial port +** @param [w] fd [int32 *] file descriptor +** +** @return [int32] success +************************************************************************/ + +int32 GPS_Command_Pvt_Off(const char *port, int32 *fd) +{ + int32 ret=0; + + + if(gps_pvt_transfer == -1) + return GPS_UNSUPPORTED; + + switch(gps_pvt_transfer) + { + case pA800: + ret = GPS_A800_Off(port,fd); + break; + default: + GPS_Error("Pvt_Off: Unknown position protocol"); + return PROTOCOL_ERROR; + } + + return ret; +} + + + +/* @func GPS_Command_Pvt_Get ******************************************** +** +** Get a single PVT info entry +** +** @param [w] fd [int32 *] file descriptor +** @param [w] pvt [GPS_PPvt_Data *] pvt data structure to fill +** +** @return [int32] success +************************************************************************/ + +int32 GPS_Command_Pvt_Get(int32 *fd, GPS_PPvt_Data *pvt) +{ + int32 ret=0; + + if(gps_pvt_transfer == -1) + return GPS_UNSUPPORTED; + + (*pvt)->fix = 0; + + switch(gps_pvt_transfer) + { + case pA800: + ret = GPS_A800_Get(fd,pvt); + break; + default: + GPS_Error("Pvt_Get: Unknown position protocol"); + return PROTOCOL_ERROR; + } + + return ret; +} diff --git a/gpsbabel/jeeps/gpscom.h b/gpsbabel/jeeps/gpscom.h new file mode 100644 index 000000000..ba1c36f09 --- /dev/null +++ b/gpsbabel/jeeps/gpscom.h @@ -0,0 +1,45 @@ +#ifdef __cplusplus +extern "C" +{ +#endif + +#ifndef gpscom_h +#define gpscom_h + + +#include "gps.h" +#include + +int32 GPS_Command_Off(const char *port); + +time_t GPS_Command_Get_Time(const char *port); +int32 GPS_Command_Send_Time(const char *port, time_t Time); + +int32 GPS_Command_Get_Position(const char *port, double *lat, double *lon); +int32 GPS_Command_Send_Position(const char *port, double lat, double lon); + +int32 GPS_Command_Pvt_On(const char *port, int32 *fd); +int32 GPS_Command_Pvt_Off(const char *port, int32 *fd); +int32 GPS_Command_Pvt_Get(int32 *fd, GPS_PPvt_Data *pvt); + +int32 GPS_Command_Get_Almanac(const char *port, GPS_PAlmanac **alm); +int32 GPS_Command_Send_Almanac(const char *port, GPS_PAlmanac *alm, int32 n); + +int32 GPS_Command_Get_Track(const char *port, GPS_PTrack **trk); +int32 GPS_Command_Send_Track(const char *port, GPS_PTrack *trk, int32 n); + +int32 GPS_Command_Get_Waypoint(const char *port, GPS_PWay **way); +int32 GPS_Command_Send_Waypoint(const char *port, GPS_PWay *way, int32 n); + +int32 GPS_Command_Get_Proximity(const char *port, GPS_PWay **way); +int32 GPS_Command_Send_Proximity(const char *port, GPS_PWay *way, int32 n); + +int32 GPS_Command_Get_Route(const char *port, GPS_PWay **way); +int32 GPS_Command_Send_Route(const char *port, GPS_PWay *way, int32 n); + + +#endif + +#ifdef __cplusplus +} +#endif diff --git a/gpsbabel/jeeps/gpsdatum.h b/gpsbabel/jeeps/gpsdatum.h new file mode 100644 index 000000000..f62405664 --- /dev/null +++ b/gpsbabel/jeeps/gpsdatum.h @@ -0,0 +1,206 @@ +#ifdef __cplusplus +extern "C" +{ +#endif + +#ifndef gpsdatum_h +#define gpsdatum_h + + + +typedef struct GPS_SEllipse +{ + char *name; + double a; + double invf; +} GPS_OEllipse, *GPS_PEllipse; + +GPS_OEllipse GPS_Ellipse[]= +{ + { "Airy 1830", 6377563.396, 299.3249646 }, + { "Airy 1830 Modified", 6377340.189, 299.3249646 }, + { "Australian National", 6378160.000, 298.25 }, + { "Bessel 1841 (Namibia)", 6377483.865, 299.1528128 }, + { "Bessel 1841", 6377397.155, 299.1528128 }, + { "Clarke 1866", 6378206.400, 294.9786982 }, + { "Clarke 1880", 6378249.145, 293.465 }, + { "Everest (India 1830)", 6377276.345, 300.8017 }, + { "Everest (Sabah Sarawak)", 6377298.556, 300.8017 }, + { "Everest (India 1956)", 6377301.243, 300.8017 }, + { "Everest (Malaysia 1969)", 6377295.664, 300.8017 }, + { "Everest (Malay & Sing)", 6377304.063, 300.8017 }, + { "Everest (Pakistan)", 6377309.613, 300.8017 }, + { "Modified Fischer 1960", 6378155.000, 298.3 }, + { "Helmert 1906", 6378200.000, 298.3 }, + { "Hough 1960", 6378270.000, 297.0 }, + { "Indonesian 1974", 6378160.000, 298.247 }, + { "International 1924", 6378388.000, 297.0 }, + { "Krassovsky 1940", 6378245.000, 298.3 }, + { "GRS67", 6378160.000, 6356774.516 }, + { "GRS75", 6378140.000, 6356755.288 }, + { "GRS80", 6378137.000, 298.257222101 }, + { "S. American 1969", 6378160.000, 298.25 }, + { "WGS60", 6378165.000, 298.3 }, + { "WGS66", 6378145.000, 298.25 }, + { "WGS72", 6378135.000, 298.26 }, + { "WGS84", 6378137.000, 298.257223563 } +}; + + + +typedef struct GPS_SDatum +{ + char *name; + int ellipse; + double dx; + double dy; + double dz; +} GPS_ODatum, *GPS_PDatum; + +GPS_ODatum GPS_Datum[]= +{ +/* 000 */ { "Adindan", 6, -166, -15, 204 }, +/* 001 */ { "AFG", 18, -43, -163, 45 }, +/* 002 */ { "Ain-El-Abd", 17, -150, -251, -2 }, +/* 003 */ { "Alaska-NAD27", 5, -5, 135, 172 }, +/* 004 */ { "Alaska-Canada", 6, -9, 151, 185 }, +/* 005 */ { "Anna-1-Astro", 2, -491, -22, 435 }, +/* 006 */ { "ARC 1950 Mean", 6, -143, -90, -294 }, +/* 007 */ { "ARC 1960 Mean", 6, -160, -8, -300 }, +/* 008 */ { "Asc Island 58", 17, -207, 107, 52 }, +/* 009 */ { "Astro B4", 17, 114, -116, -333 }, +/* 010 */ { "Astro Beacon E", 17, 145, 75, -272 }, +/* 011 */ { "Astro pos 71/4", 17, -320, 550, -494 }, +/* 012 */ { "Astro stn 52", 17, 124, -234, -25 }, +/* 013 */ { "Australia Geo 1984", 2, -134, -48, 149 }, +/* 014 */ { "Bahamas NAD27", 6, -4, 154, 178 }, +/* 015 */ { "Bellevue IGN", 17, -127, -769, 472 }, +/* 016 */ { "Bermuda 1957", 6, -73, 213, 296 }, +/* 017 */ { "Bukit Rimpah", 4, -384, 664, -48 }, +/* 018 */ { "Camp_Area_Astro", 17, -104, -129, 239 }, +/* 019 */ { "Campo_Inchauspe", 17, -148, 136, 90 }, +/* 020 */ { "Canada_Mean(NAD27)", 5, -10, 158, 187 }, +/* 021 */ { "Canal_Zone_(NAD27)", 5, 0, 125, 201 }, +/* 022 */ { "Canton_Island_1966", 17, 298, -304, -375 }, +/* 023 */ { "Cape", 6, -136, -108, -292 }, +/* 024 */ { "Cape_Canaveral_mean", 5, -2, 150, 181 }, +/* 025 */ { "Carribean NAD27", 5, -7, 152, 178 }, +/* 026 */ { "Carthage", 6, -263, 6, 431 }, +/* 027 */ { "Cent America NAD27", 5 , 0, 125, 194 }, +/* 028 */ { "Chatham 1971", 17, 175, -38, 113 }, +/* 029 */ { "Chua Astro", 17, -134, 229, -29 }, +/* 030 */ { "Corrego Alegre", 17, -206, 172, -6 }, +/* 031 */ { "Cuba NAD27", 5, -9, 152, 178 }, +/* 032 */ { "Cyprus", 17, -104, -101, -140 }, +/* 033 */ { "Djakarta(Batavia)", 4, -377, 681, -50 }, +/* 034 */ { "DOS 1968", 17, 230, -199, -752 }, +/* 035 */ { "Easter lsland 1967", 17, 211, 147, 111 }, +/* 036 */ { "Egypt", 17, -130, -117, -151 }, +/* 037 */ { "European 1950", 17, -87, -96, -120 }, +/* 038 */ { "European 1950 mean", 17, -87, -98, -121 }, +/* 039 */ { "European 1979 mean", 17, -86, -98, -119 }, +/* 040 */ { "Finnish Nautical", 17, -78, -231, -97 }, +/* 041 */ { "Gandajika Base", 17, -133, -321, 50 }, +/* 042 */ { "Geodetic Datum 49", 17, 84, -22, 209 }, +/* 043 */ { "Ghana", 26, 0, 0, 0 }, +/* 044 */ { "Greenland NAD27", 5, 11, 114, 195 }, +/* 045 */ { "Guam 1963", 5, -100, -248, 259 }, +/* 046 */ { "Gunung Segara", 4, -403, 684, 41 }, +/* 047 */ { "Gunung Serindung 1962", 26, 0, 0, 0 }, +/* 048 */ { "GUX1 Astro", 17, 252, -209, -751 }, +/* 049 */ { "Herat North", 17, -333, -222, 114 }, +/* 050 */ { "Hjorsey 1955", 17, -73, 46, 86 }, +/* 051 */ { "Hong Kong 1963", 17, -156, -271, -189 }, +/* 052 */ { "Hu-Tzu-Shan", 17, -634, -549, -201 }, +/* 053 */ { "Indian", 9, 289, 734, 257 }, +/* 054 */ { "Iran", 17, -117, -132, -164 }, +/* 055 */ { "Ireland 1965", 1, 506, -122, 611 }, +/* 056 */ { "ISTS 073 Astro 69", 17, 208, -435, -229 }, +/* 057 */ { "Johnston Island 61", 17, 191, -77, -204 }, +/* 058 */ { "Kandawala", 7, -97, 787, 86 }, +/* 059 */ { "Kerguelen Island", 17, 145, -187, 103 }, +/* 060 */ { "Kertau 48", 11, -11, 851, 5 }, +/* 061 */ { "L.C. 5 Astro", 5, 42, 124, 147 }, +/* 062 */ { "La Reunion", 17, 94, -948, -1262 }, +/* 063 */ { "Liberia 1964", 6, -90, 40, 88 }, +/* 064 */ { "Luzon", 5, -133, -77, -51 }, +/* 065 */ { "Mahe 1971", 6, 41, -220, -134 }, +/* 066 */ { "Marco Astro", 17, -289, -124, 60 }, +/* 067 */ { "Masirah Is. Nahrwan", 6, -247, -148, 369 }, +/* 068 */ { "Massawa", 4, 639, 405, 60 }, +/* 069 */ { "Merchich", 6, 31, 146, 47 }, +/* 070 */ { "Mexico NAD27", 5, -12, 130, 190 }, +/* 071 */ { "Midway Astro 61", 17, 912, -58, 1227 }, +/* 072 */ { "Mindanao", 5, -133, -79, -72 }, +/* 073 */ { "Minna", 6, -92, -93, 122 }, +/* 074 */ { "Montjong Lowe", 26, 0, 0, 0 }, +/* 075 */ { "Nahrwan", 6, -231, -196, 482 }, +/* 076 */ { "Naparima BWI", 17, -2, 374, 172 }, +/* 077 */ { "North America 83", 21, 0, 0, 0 }, +/* 078 */ { "N. America 1927 mean", 5, -8, 160, 176 }, +/* 079 */ { "Observatorio 1966", 17, -425, -169, 81 }, +/* 080 */ { "Old Egyptian", 14, -130, 110, -13 }, +/* 081 */ { "Old Hawaiian_mean", 5, 89, -279, -183 }, +/* 082 */ { "Old Hawaiian Kauai", 5, 45, -290, -172 }, +/* 083 */ { "Old Hawaiian Maui", 5, 65, -290, -190 }, +/* 084 */ { "Old Hawaiian Oahu", 5, 56, -284, -181 }, +/* 085 */ { "Oman", 6, -346, -1, 224 }, +/* 086 */ { "OSGB36", 0, 375, -111, 431 }, +/* 087 */ { "Pico De Las Nieves", 17, -307, -92, 127 }, +/* 088 */ { "Pitcairn Astro 67", 17, 185, 165, 42 }, +/* 089 */ { "S. Am. 1956 mean(P)", 17, -288, 175, -376 }, +/* 090 */ { "S. Chilean 1963 (P)", 17, 16, 196, 93 }, +/* 091 */ { "Puerto Rico", 5, 11, 72, -101 }, +/* 092 */ { "Pulkovo 1942", 18, 28, -130, -95 }, +/* 093 */ { "Qornoq", 17, 164, 138, -189 }, +/* 094 */ { "Quatar National", 17, -128, -283, 22 }, +/* 095 */ { "Rome 1940", 17, -225, -65, 9 }, +/* 096 */ { "S-42(Pulkovo1942)", 18, 28, -121, -77 }, +/* 097 */ { "S.E.Asia_(Indian)", 7, 173, 750, 264 }, +/* 098 */ { "SAD-69/Brazil", 22, -60, -2, -41 }, +/* 099 */ { "Santa Braz", 17, -203, 141, 53 }, +/* 100 */ { "Santo (DOS)", 17, 170, 42, 84 }, +/* 101 */ { "Sapper Hill 43", 17, -355, 16, 74 }, +/* 102 */ { "Schwarzeck", 3, 616, 97, -251 }, +/* 103 */ { "Sicily", 17, -97, -88, -135 }, +/* 104 */ { "Sierra Leone 1960", 26, 0, 0, 0 }, +/* 105 */ { "S. Am. 1969 mean", 22, -57, 1, -41 }, +/* 106 */ { "South Asia", 13, 7, -10, -26 }, +/* 107 */ { "Southeast Base", 17, -499, -249, 314 }, +/* 108 */ { "Southwest Base", 17, -104, 167, -38 }, +/* 109 */ { "Tananarive Obs 25", 17, -189, -242, -91 }, +/* 110 */ { "Thai/Viet (Indian)", 7, 214, 836, 303 }, +/* 111 */ { "Timbalai 1948", 7, -689, 691, -45 }, +/* 112 */ { "Tokyo mean", 4, -128, 481, 664 }, +/* 113 */ { "Tristan Astro 1968", 17, -632, 438, -609 }, +/* 114 */ { "Unites Arab Emirates", 6, -249, -156, 381 }, +/* 115 */ { "Viti Levu 1916", 6, 51, 391, -36 }, +/* 116 */ { "Wake Eniwetok 60", 15, 101, 52, -39 }, +/* 117 */ { "WGS 72", 25, 0, 0, 5 }, +/* 118 */ { "WGS 84", 26, 0, 0, 0 }, +/* 119 */ { "Yacare", 17, -155, 171, 37 }, +/* 120 */ { "Zanderij", 17, -265, 120, -358 }, +/* 121 */ { "Sweden", 4, 424.3, -80.5, 613.1 }, + { NULL, 0, 0, 0, 0 } +}; + + +/* UK Ordnance Survey Nation Grid Map Codes */ +static char *UKNG[]= +{ + "SV","SW","SX","SY","SZ","TV","TW","SQ","SR","SS","ST","SU","TQ","TR", + "SL","SM","SN","SO","SP","TL","TM","SF","SG","SH","SJ","SK","TF","TG", + "SA","SB","SC","SD","SE","TA","TB","NV","NW","NX","NY","NZ","OV","OW", + "NQ","NR","NS","NT","NU","OQ","OR","NL","NM","NN","NO","NP","OL","OM", + "NF","NG","NH","NJ","NK","OF","OG","NA","NB","NC","ND","NE","OA","OB", + "HV","HW","HX","HY","HZ","JV","JW","HQ","HR","HS","HT","HU","JQ","JR", + "HL","HM","HN","HO","HP","JL","JM","" +}; + + + +#endif + +#ifdef __cplusplus +} +#endif diff --git a/gpsbabel/jeeps/gpsfmt.c b/gpsbabel/jeeps/gpsfmt.c new file mode 100644 index 000000000..4164fa60b --- /dev/null +++ b/gpsbabel/jeeps/gpsfmt.c @@ -0,0 +1,1468 @@ +/******************************************************************** +** @source JEEPS output functions +** +** @author Copyright (C) 1999 Alan Bleasby +** @version 1.0 +** @modified Dec 28 1999 Alan Bleasby. First version +** @@ +** +** This library is free software; you can redistribute it and/or +** modify it under the terms of the GNU Library General Public +** License as published by the Free Software Foundation; either +** version 2 of the License, or (at your option) any later version. +** +** This library is distributed in the hope that it will be useful, +** but WITHOUT ANY WARRANTY; without even the implied warranty of +** MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU +** Library General Public License for more details. +** +** You should have received a copy of the GNU Library General Public +** License along with this library; if not, write to the +** Free Software Foundation, Inc., 59 Temple Place - Suite 330, +** Boston, MA 02111-1307, USA. +********************************************************************/ +#include "gps.h" +#include +#include + + +static void GPS_Fmt_Print_Way100(GPS_PWay way, FILE *outf); +static void GPS_Fmt_Print_Way101(GPS_PWay way, FILE *outf); +static void GPS_Fmt_Print_Way102(GPS_PWay way, FILE *outf); +static void GPS_Fmt_Print_Way103(GPS_PWay way, FILE *outf); +static void GPS_Fmt_Print_Way104(GPS_PWay way, FILE *outf); +static void GPS_Fmt_Print_Way105(GPS_PWay way, FILE *outf); +static void GPS_Fmt_Print_Way106(GPS_PWay way, FILE *outf); +static void GPS_Fmt_Print_Way107(GPS_PWay way, FILE *outf); +static void GPS_Fmt_Print_Way108(GPS_PWay way, FILE *outf); +static void GPS_Fmt_Print_Way150(GPS_PWay way, FILE *outf); +static void GPS_Fmt_Print_Way151(GPS_PWay way, FILE *outf); +static void GPS_Fmt_Print_Way152(GPS_PWay way, FILE *outf); +static void GPS_Fmt_Print_Way154(GPS_PWay way, FILE *outf); +static void GPS_Fmt_Print_Way155(GPS_PWay way, FILE *outf); + +static void GPS_Fmt_Print_Track301(GPS_PTrack *trk, int32 n, FILE *outf); +static void GPS_Fmt_Print_D300(GPS_PTrack trk, FILE *outf); +static void GPS_Fmt_Print_D301(GPS_PTrack trk, FILE *outf); + +static int32 GPS_Fmt_Print_Route201(GPS_PWay *way, int32 n, FILE *outf); + + +char *gps_marine_sym[]= +{ + "Anchor","Bell","Diamond-grn","Diamond_red","Dive1","Dive2","Dollar", + "Fish","Fuel","Horn","House","Knife","Light","Mug","Skull", + "Square_grn","Square_red","Wbuoy","Wpt_dot","Wreck","Null","Mob", + + "Buoy_amber","Buoy_blck","Buoy_blue","Buoy_grn","Buoy_grn_red", + "Buoy_grn_wht","Buoy_orng","Buoy_red","Buoy_red_grn","Buoy_red_wht", + "Buoy_violet","Buoy_wht","Buoy_wht_grn","Buoy_wht_red","Dot","Rbcn", + + "","","","","","","","","","", + "","","","","","","","","","", + "","","","","","","","","","", + "","","","","","","","","","", + "","","","","","","","","","", + "","","","","","","","","","", + "","","","","","","","","","", + "","","","","","","","","","", + "","","","","","","","","","", + "","","","","","","","","","", + "","","","","","","","","","", + "","", + + "Boat_ramp","Camp","Toilets","Showers","Drinking_wtr","Phone", + "1st_aid","Info","Parking","Park","Picnic","Scenic","Skiing", + "Swimming","Dam","Controlled","Danger","Restricted","Null_2","Ball", + "Car","Deer","Shpng_trolley","Lodging","Mine","Trail_head", + "Lorry_stop","User_exit","Flag","Circle-x" +}; + + + +char *gps_land_sym[]= +{ + "Is_hwy","Us_hwy","St_hwy","Mi_mrkr","Trcbck","Golf","Sml_cty", + "Med_cty","Lrg_cty","Freeway","Ntl_hwy","Cap_cty","Amuse_pk", + "Bowling","Car_rental","Car_repair","Fastfood","Fitness","Film", + "Museum","Chemist","Pizza","Post_ofc","Rv_park","School", + "Stadium","Shop","Zoo","Petrol_plus","Theatre","Ramp_int", + "St_int","","","Weigh_stn","Toll_booth","Elev_pt","Ex_no_srvc", + "Geo_place_mm","Geo_place_wtr","Geo_place_lnd","Bridge","Building", + "Cemetery","Church","Civil_loc","Crossing","Hist_town","River_Embankment", + "Military_loc","Oil_field","Tunnel","Beach","Forest","Summit", + "Lrg_ramp_int","Lrg_exit_no_srvc","Official_badge","Gambling", + "Snow_ski","Ice_ski","Tow_truck","Border" +}; + + +char *gps_aviation_sym[]= +{ + "Airport","Int","Ndb","Vor","Heliport","Private","Soft_fld", + "Tall_tower","Short_tower","Glider","Ultralight","Parachute", + "Vortac","Vordme","Faf","Lom","Map","Tacan","Seaplane" +}; + + +char *gps_16_sym[]= +{ + "Dot","House","Fuel","Car","Fish","Boat","Anchor","Wreck", + "Exit","Skull","Flag","Camp","Circle-x","Deer","1st_aid","Back_track" +}; + + + + + +/* @func GPS_Fmt_Print_Time ******************************************** +** +** Output Date/time +** +** @param [r] Time [time_t] unix-style time +** @param [w] outf [FILE *] output stream +** +** @return [void] +************************************************************************/ + +void GPS_Fmt_Print_Time(time_t Time, FILE *outf) +{ + (void) fprintf(outf,"%s",ctime(&Time)); + fflush(outf); + + return; +} + + + +/* @func GPS_Fmt_Print_Position ******************************************** +** +** Output position +** +** @param [r] lat [double] latitude (deg) +** @param [r] lon [double] longitude (deg) +** @param [w] outf [FILE *] output stream +** +** @return [void] +************************************************************************/ + +void GPS_Fmt_Print_Position(double lat, double lon, FILE *outf) +{ + (void) fprintf(outf,"Latitude: %f Longitude %f\n",lat,lon); + fflush(outf); + + return; +} + + + +/* @func GPS_Fmt_Print_Pvt ******************************************** +** +** Output pvt +** +** @param [r] pvt [GPS_PPvt_Data] pvt +** @param [w] outf [FILE *] output stream +** +** @return [void] +************************************************************************/ + +void GPS_Fmt_Print_Pvt(GPS_PPvt_Data pvt, FILE *outf) +{ + + (void) fprintf(outf,"Fix: "); + switch(pvt->fix) + { + case 0: + (void) fprintf(outf,"UNUSABLE\n\n"); + break; + case 1: + (void) fprintf(outf,"INVALID \n\n"); + break; + case 2: + (void) fprintf(outf,"2D \n\n"); + break; + case 3: + (void) fprintf(outf,"3D \n\n"); + break; + case 4: + (void) fprintf(outf,"2D-diff \n\n"); + break; + case 5: + (void) fprintf(outf,"2D-diff \n\n"); + break; + default: + (void) fprintf(stderr,"PVT: Unsupported Fix type\n"); + break; + } + + (void) fprintf(outf,"Altitude (WGS 84): %-20f \n",pvt->alt); + (void) fprintf(outf,"EPE: %-20f \n",pvt->epe); + (void) fprintf(outf,"EPE (hor only): %-20f \n",pvt->eph); + (void) fprintf(outf,"EPE (ver only): %-20f \n",pvt->epv); + (void) fprintf(outf,"Time of week: %-20d \n",(int)pvt->tow); + (void) fprintf(outf,"Latitude: %-20f \n",pvt->lat); + (void) fprintf(outf,"Longitude: %-20f \n",pvt->lon); + (void) fprintf(outf,"East velocity: %-20f \n",pvt->east); + (void) fprintf(outf,"North velocity: %-20f \n",pvt->north); + (void) fprintf(outf,"Upward velocity %-20f \n",pvt->up); + (void) fprintf(outf,"Height above MSL: %-20f \n",pvt->msl_hght+pvt->alt); + (void) fprintf(outf,"Leap seconds: %-20d \n",pvt->leap_scnds); + (void) fprintf(outf,"Week number days: %-20d \n",(int)pvt->wn_days); + + fflush(outf); + + return; +} + + + +/* @func GPS_Fmt_Print_Almanac ******************************************** +** +** Output almanac +** +** @param [r] alm [GPS_PAlmanac *] almanac array +** @param [r] n [int32] number of almanac entries +** @param [w] outf [FILE *] output stream +** +** @return [void] +************************************************************************/ + +void GPS_Fmt_Print_Almanac(GPS_PAlmanac *alm, int32 n, FILE *outf) +{ + int32 i; + int32 t; + int32 s; + + /* Type 0 models require all 32 satellites to be sent */ + t=32; + s=0; + if(n && alm[0]->svid!=0xff) + { + s=1; + t=n; + } + (void) fprintf(outf,"Almanac %d %d\n",(int)t,(int)s); + + + for(i=0;iwn<0) continue; + + if(alm[i]->svid == 0xff) + alm[i]->svid = i; + (void) fprintf(outf,"#\n#\n"); + (void) fprintf(outf,"\tID: %d\n", + alm[i]->svid+1); + (void) fprintf(outf,"\tWeek number: %d\n", + alm[i]->wn); + (void) fprintf(outf,"\tAlmanac Data Reference Time: %f\n", + alm[i]->toa); + (void) fprintf(outf,"\tClock Correction Coeff (s): %f\n", + alm[i]->af0); + (void) fprintf(outf,"\tClock Correction Coeff (s/s): %f\n", + alm[i]->af1); + (void) fprintf(outf,"\tEccentricity: %f\n", + alm[i]->e); + (void) fprintf(outf,"\tSqrt of semi-major axis: %f\n", + alm[i]->sqrta); + (void) fprintf(outf,"\tMean Anomaly at Ref. Time: %f\n", + alm[i]->m0); + (void) fprintf(outf,"\tArgument of perigee: %f\n", + alm[i]->w); + (void) fprintf(outf,"\tRight ascension: %f\n", + alm[i]->omg0); + (void) fprintf(outf,"\tRate of right ascension: %f\n", + alm[i]->odot); + (void) fprintf(outf,"\tInclination angle: %f\n", + alm[i]->i); + (void) fprintf(outf,"\tHealth: %d\n", + alm[i]->hlth); + } + + + fflush(outf); + + return; +} + + + +/* @func GPS_Fmt_Print_Track ******************************************** +** +** Output track log +** +** @param [r] trk [GPS_PTrack *] track array +** @param [r] n [int32] number of track entries +** @param [w] outf [FILE *] output stream +** +** @return [void] +************************************************************************/ + +void GPS_Fmt_Print_Track(GPS_PTrack *trk, int32 n, FILE *outf) +{ + int32 i; + + + switch(gps_trk_transfer) + { + case pA300: + break; + case pA301: + GPS_Fmt_Print_Track301(trk,n,outf); + return; + default: + GPS_Error("GPS_Fmt_Print_Track: Unknown protocol"); + return; + } + + + (void) fprintf(outf,"Track log 300 %d\n#\n",(int)gps_trk_type); + (void) fprintf(outf,"Start\n#\n"); + + for(i=0;itnew) + (void) fprintf(outf,"#\nNew track\n#\n"); + + switch(gps_trk_type) + { + case pD300: + GPS_Fmt_Print_D300(trk[i],outf); + break; + case pD301: + GPS_Fmt_Print_D301(trk[i],outf); + break; + default: + break; + } + } + + (void) fprintf(outf,"End\n#\n"); + fflush(outf); + + return; +} + + + +/* @funcstatic GPS_Fmt_Print_Track301 *********************************** +** +** Output track log +** +** @param [r] trk [GPS_PTrack *] track array +** @param [r] n [int32] number of track entries +** @param [w] outf [FILE *] output stream +** +** @return [void] +************************************************************************/ + +static void GPS_Fmt_Print_Track301(GPS_PTrack *trk, int32 n, FILE *outf) +{ + int32 i; + + if(!n) + return; + + (void) fprintf(outf,"Track log 301 %d\n#\n",(int)gps_trk_type); + (void) fprintf(outf,"Start\n#\n"); + + for(i=0;iishdr) + { + (void) fprintf(outf,"Header\n"); + (void) fprintf(outf,"\tIdent: %s\n",trk[i]->trk_ident); + (void) fprintf(outf,"\tDisplay: %d\n",(int)trk[i]->dspl); + (void) fprintf(outf,"\tColour: %d\n#\n", + (int)trk[i]->colour); + continue; + } + + if(trk[i]->tnew) + (void) fprintf(outf,"#\nNew track\n#\n"); + + switch(gps_trk_type) + { + case pD300: + GPS_Fmt_Print_D300(trk[i],outf); + break; + case pD301: + GPS_Fmt_Print_D301(trk[i],outf); + break; + default: + break; + } + } + + (void) fprintf(outf,"End\n#\n"); + fflush(outf); + + return; +} + + +/* @funcstatic GPS_Fmt_Print_D300 **************************************** +** +** Output track log +** +** @param [r] trk [GPS_PTrack *] track array +** @param [w] outf [FILE *] output stream +** +** @return [void] +************************************************************************/ + +static void GPS_Fmt_Print_D300(GPS_PTrack trk, FILE *outf) +{ + (void) fprintf(outf,"\tLatitude: %f\n",trk->lat); + (void) fprintf(outf,"\tLongitude: %f\n",trk->lon); + if(trk->Time) + (void) fprintf(outf,"\tTime: %s\n",ctime(&trk->Time)); + else + (void) fprintf(outf,"\tTime: Computer\n\n"); + + return; +} + + + +/* @funcstatic GPS_Fmt_Print_D301 **************************************** +** +** Output track log +** +** @param [r] trk [GPS_PTrack *] track array +** @param [w] outf [FILE *] output stream +** +** @return [void] +************************************************************************/ + +static void GPS_Fmt_Print_D301(GPS_PTrack trk, FILE *outf) +{ + (void) fprintf(outf,"\tLatitude: %f\n",trk->lat); + (void) fprintf(outf,"\tLongitude: %f\n",trk->lon); + if(trk->Time) + (void) fprintf(outf,"\tTime: %s",ctime(&trk->Time)); + else + (void) fprintf(outf,"\tTime: Computer\n"); + (void) fprintf(outf,"\tAltitude: %f\n",trk->alt); + (void) fprintf(outf,"\tDepth: %f\n\n",trk->dpth); + + return; +} + + + + +/* @func GPS_Fmt_Print_Waypoint ***************************************** +** +** Output waypoints +** +** @param [r] way [GPS_PWay *] waypoint array +** @param [r] n [int32] number of waypoint entries +** @param [w] outf [FILE *] output stream +** +** @return [int32] success +************************************************************************/ + +int32 GPS_Fmt_Print_Waypoint(GPS_PWay *way, int32 n, FILE *outf) +{ + int32 i; + + + if(!n) + return 1; + + (void) fprintf(outf,"Waypoints Type: %d\n#\nStart\n#\n", + (int)way[0]->prot); + + + for(i=0;iprot) + { + case 100: + GPS_Fmt_Print_Way100(way[i],outf); + break; + case 101: + GPS_Fmt_Print_Way101(way[i],outf); + break; + case 102: + GPS_Fmt_Print_Way102(way[i],outf); + break; + case 103: + GPS_Fmt_Print_Way103(way[i],outf); + break; + case 104: + GPS_Fmt_Print_Way104(way[i],outf); + break; + case 105: + GPS_Fmt_Print_Way105(way[i],outf); + break; + case 106: + GPS_Fmt_Print_Way106(way[i],outf); + break; + case 107: + GPS_Fmt_Print_Way107(way[i],outf); + break; + case 108: + GPS_Fmt_Print_Way108(way[i],outf); + break; + case 150: + GPS_Fmt_Print_Way150(way[i],outf); + break; + case 151: + GPS_Fmt_Print_Way151(way[i],outf); + break; + case 152: + GPS_Fmt_Print_Way152(way[i],outf); + break; + case 154: + GPS_Fmt_Print_Way154(way[i],outf); + break; + case 155: + GPS_Fmt_Print_Way155(way[i],outf); + break; + default: + GPS_Error("Print_Waypoint: Unknown waypoint protocol"); + return PROTOCOL_ERROR; + } + (void) fprintf(outf,"#\n"); + } + (void) fprintf(outf,"End\n#\n"); + + return 1; +} + + + +/* @func GPS_Fmt_Print_Proximity ***************************************** +** +** Output proximity +** +** @param [r] way [GPS_PWay *] waypoint array +** @param [r] n [int32] number of waypoint entries +** @param [w] outf [FILE *] output stream +** +** @return [int32] success +************************************************************************/ + +int32 GPS_Fmt_Print_Proximity(GPS_PWay *way, int32 n, FILE *outf) +{ + int32 i; + + + if(!n) + return 1; + + (void) fprintf(outf,"Waypoints Type: %d\n#\nStart\n#\n", + (int)way[0]->prot); + + + for(i=0;iprot) + { + case 400: + GPS_Fmt_Print_Way100(way[i],outf); + break; + case 101: + GPS_Fmt_Print_Way101(way[i],outf); + break; + case 102: + GPS_Fmt_Print_Way102(way[i],outf); + break; + case 403: + GPS_Fmt_Print_Way103(way[i],outf); + break; + case 104: + GPS_Fmt_Print_Way104(way[i],outf); + break; + case 105: + GPS_Fmt_Print_Way105(way[i],outf); + break; + case 106: + GPS_Fmt_Print_Way106(way[i],outf); + break; + case 107: + GPS_Fmt_Print_Way107(way[i],outf); + break; + case 108: + GPS_Fmt_Print_Way108(way[i],outf); + break; + case 450: + GPS_Fmt_Print_Way150(way[i],outf); + (void) fprintf(outf,"\tPindex: %d\n",(int)way[i]->idx); + break; + case 151: + GPS_Fmt_Print_Way151(way[i],outf); + break; + case 152: + GPS_Fmt_Print_Way152(way[i],outf); + break; + case 154: + GPS_Fmt_Print_Way154(way[i],outf); + break; + case 155: + GPS_Fmt_Print_Way155(way[i],outf); + break; + default: + GPS_Error("Print_Proximity: Unknown proximity protocol"); + return PROTOCOL_ERROR; + } + (void) fprintf(outf,"\tDistance: %f\n",way[i]->dst); + (void) fprintf(outf,"#\n"); + } + (void) fprintf(outf,"End\n#\n"); + + return 1; +} + + + + +/* @funcstatic GPS_Fmt_Print_Way100 ************************************* +** +** Output waypoint D100 +** +** @param [r] way [GPS_PWay] waypoint +** @param [w] outf [FILE *] output stream +** +** @return [void] +************************************************************************/ + +static void GPS_Fmt_Print_Way100(GPS_PWay way, FILE *outf) +{ + + (void) fprintf(outf,"\tIdent: %-6.6s\n",way->ident); + (void) fprintf(outf,"\tLatitude: %f\n",way->lat); + (void) fprintf(outf,"\tLongitude: %f\n",way->lon); + (void) fprintf(outf,"\tComment: %-40.40s\n",way->cmnt); + + return; +} + + +/* @funcstatic GPS_Fmt_Print_Way101 ************************************ +** +** Output waypoint D101 +** +** @param [r] way [GPS_PWay] waypoint +** @param [w] outf [FILE *] output stream +** +** @return [void] +************************************************************************/ + +static void GPS_Fmt_Print_Way101(GPS_PWay way, FILE *outf) +{ + + if(way->smbl > 176) way->smbl=36; + + (void) fprintf(outf,"\tIdent: %-6.6s\n",way->ident); + (void) fprintf(outf,"\tLatitude: %f\n",way->lat); + (void) fprintf(outf,"\tLongitude: %f\n",way->lon); + (void) fprintf(outf,"\tComment: %-40.40s\n",way->cmnt); + (void) fprintf(outf,"\tSymbol: %d [%s]\n",(int)way->smbl, + gps_marine_sym[way->smbl]); + + return; +} + + +/* @funcstatic GPS_Fmt_Print_Way102 ************************************* +** +** Output waypoint D102 +** +** @param [r] way [GPS_PWay] waypoint +** @param [w] outf [FILE *] output stream +** +** @return [void] +************************************************************************/ + +static void GPS_Fmt_Print_Way102(GPS_PWay way, FILE *outf) +{ + char **p; + int32 x; + + if(way->smbl < 8192) + { + p = gps_marine_sym; + x = 0; + } + else if(way->smbl < 16384) + { + p = gps_land_sym; + x = 8192; + } + else + { + p = gps_aviation_sym; + x = 16384; + } + + + (void) fprintf(outf,"\tIdent: %-6.6s\n",way->ident); + (void) fprintf(outf,"\tLatitude: %f\n",way->lat); + (void) fprintf(outf,"\tLongitude: %f\n",way->lon); + (void) fprintf(outf,"\tComment: %-40.40s\n",way->cmnt); + (void) fprintf(outf,"\tSymbol: %d [%s]\n",(int)way->smbl, + p[way->smbl-x]); + + return; +} + + +/* @funcstatic GPS_Fmt_Print_Way103 ************************************ +** +** Output waypoint D103 +** +** @param [r] way [GPS_PWay] waypoint +** @param [w] outf [FILE *] output stream +** +** @return [void] +************************************************************************/ + +static void GPS_Fmt_Print_Way103(GPS_PWay way, FILE *outf) +{ + static char *dspl[]= + { + "SW","S","SC" + }; + + (void) fprintf(outf,"\tIdent: %-6.6s\n",way->ident); + (void) fprintf(outf,"\tLatitude: %f\n",way->lat); + (void) fprintf(outf,"\tLongitude: %f\n",way->lon); + (void) fprintf(outf,"\tComment: %-40.40s\n",way->cmnt); + (void) fprintf(outf,"\tSymbol: %-6d [%s]\n",(int)way->smbl, + gps_16_sym[way->smbl]); + (void) fprintf(outf,"\tDisplay: %-6d [%s]\n",(int)way->dspl, + dspl[way->dspl]); + + return; +} + + +/* @funcstatic GPS_Fmt_Print_Way104 ************************************* +** +** Output waypoint D104 +** +** @param [r] way [GPS_PWay] waypoint +** @param [w] outf [FILE *] output stream +** +** @return [void] +************************************************************************/ + +static void GPS_Fmt_Print_Way104(GPS_PWay way, FILE *outf) +{ + static char *dspl[]= + { + "S","S","","SW","","SC" + }; + char **p; + int32 x; + + if(way->smbl < 8192) + { + p = gps_marine_sym; + x = 0; + } + else if(way->smbl < 16384) + { + p = gps_land_sym; + x = 8192; + } + else + { + p = gps_aviation_sym; + x = 16384; + } + + (void) fprintf(outf,"\tIdent: %-6.6s\n",way->ident); + (void) fprintf(outf,"\tLatitude: %f\n",way->lat); + (void) fprintf(outf,"\tLongitude: %f\n",way->lon); + (void) fprintf(outf,"\tComment: %-40.40s\n",way->cmnt); + (void) fprintf(outf,"\tSymbol: %-6d [%s]\n",(int)way->smbl, + p[way->smbl-x]); + (void) fprintf(outf,"\tDisplay: %-6d [%s]\n",(int)way->dspl, + dspl[way->dspl]); + + return; +} + + +/* @funcstatic GPS_Fmt_Print_Way105 ************************************ +** +** Output waypoint D105 +** +** @param [r] way [GPS_PWay] waypoint +** @param [w] outf [FILE *] output stream +** +** @return [void] +************************************************************************/ + +static void GPS_Fmt_Print_Way105(GPS_PWay way, FILE *outf) +{ + char **p; + int32 x; + + if(way->smbl < 8192) + { + p = gps_marine_sym; + x = 0; + } + else if(way->smbl < 16384) + { + p = gps_land_sym; + x = 8192; + } + else + { + p = gps_aviation_sym; + x = 16384; + } + + (void) fprintf(outf,"\tLatitude: %f\n",way->lat); + (void) fprintf(outf,"\tLongitude: %f\n",way->lon); + (void) fprintf(outf,"\tSymbol: %-6d [%s]\n",(int)way->smbl, + p[way->smbl-x]); + (void) fprintf(outf,"\tWpt_ident %s\n",way->wpt_ident); + + return; +} + + +/* @funcstatic GPS_Fmt_Print_Way106 ************************************* +** +** Output waypoint D106 +** +** @param [r] way [GPS_PWay] waypoint +** @param [w] outf [FILE *] output stream +** +** @return [void] +************************************************************************/ + +static void GPS_Fmt_Print_Way106(GPS_PWay way, FILE *outf) +{ + char **p; + int32 x; + + if(way->smbl < 8192) + { + p = gps_marine_sym; + x = 0; + } + else if(way->smbl < 16384) + { + p = gps_land_sym; + x = 8192; + } + else + { + p = gps_aviation_sym; + x = 16384; + } + + if(!way->wpt_class) + { + (void) fprintf(outf,"\tClass: %d [User]\n",way->wpt_class); + (void) fprintf(outf,"\tSubclass: %d [%-13.13s]\n", + way->wpt_class,way->subclass); + (void) fprintf(outf,"\tSubclass:\n"); + } + else + { + (void) fprintf(outf,"\tClass: %d [Non-user]\n", + way->wpt_class); + (void) fprintf(outf,"\tSubclass: %d [%13.13s]\n", + way->wpt_class, + way->subclass); + } + (void) fprintf(outf,"\tLatitude: %f\n",way->lat); + (void) fprintf(outf,"\tLongitude: %f\n",way->lon); + (void) fprintf(outf,"\tSymbol: %-6d [%s]\n",(int)way->smbl, + p[way->smbl-x]); + (void) fprintf(outf,"\tWpt_ident %s\n",way->wpt_ident); + (void) fprintf(outf,"\tLnk_ident %s\n",way->lnk_ident); + + return; +} + + +/* @funcstatic GPS_Fmt_Print_Way107 ************************************ +** +** Output waypoint D107 +** +** @param [r] way [GPS_PWay] waypoint +** @param [w] outf [FILE *] output stream +** +** @return [void] +************************************************************************/ + +static void GPS_Fmt_Print_Way107(GPS_PWay way, FILE *outf) +{ + static char *dspl[]= + { + "SW","S","SC" + }; + static char *col[]= + { + "Default","Red","Green","Blue" + }; + + + (void) fprintf(outf,"\tIdent: %-6.6s\n",way->ident); + (void) fprintf(outf,"\tLatitude: %f\n",way->lat); + (void) fprintf(outf,"\tLongitude: %f\n",way->lon); + (void) fprintf(outf,"\tComment: %-40.40s\n",way->cmnt); + (void) fprintf(outf,"\tSymbol: %-6d [%s]\n",(int)way->smbl, + gps_16_sym[way->smbl]); + (void) fprintf(outf,"\tDisplay: %-6d [%s]\n",(int)way->dspl, + dspl[way->dspl]); + (void) fprintf(outf,"\tColour: %-6d [%s]\n",(int)way->colour, + col[way->colour]); + + return; +} + + + +/* @funcstatic GPS_Fmt_Print_Way108 ************************************ +** +** Output waypoint D108 +** +** @param [r] way [GPS_PWay] waypoint +** @param [w] outf [FILE *] output stream +** +** @return [void] +************************************************************************/ + +static void GPS_Fmt_Print_Way108(GPS_PWay way, FILE *outf) +{ + char **p; + int32 x; + + static char *dspl[]= + { + "SW","S","SC" + }; + + static char *col[]= + { + "Black","Dark_Red","Dark_Green","Dark_Yellow","Dark_Blue", + "Dark_Magenta","Dark_Cyan","Light_Grey","Dark_Grey","Red","Green", + "Yellow","Blue","Magenta","Cyan","White" + }; + + + if(way->smbl < 8192) + { + p = gps_marine_sym; + x = 0; + } + else if(way->smbl < 16384) + { + p = gps_land_sym; + x = 8192; + } + else + { + p = gps_aviation_sym; + x = 16384; + } + + (void) fprintf(outf,"\tIdent: %s\n",way->ident); + (void) fprintf(outf,"\tLatitude: %f\n",way->lat); + (void) fprintf(outf,"\tLongitude: %f\n",way->lon); + if(way->colour==0xff) + (void) fprintf(outf,"\tColour: 255 [Default]\n"); + else + (void) fprintf(outf,"\tColour: %-6d [%s]\n",(int)way->colour, + col[way->colour]); + (void) fprintf(outf,"\tDisplay: %-6d [%s]\n",(int)way->dspl, + dspl[way->dspl]); + (void) fprintf(outf,"\tSymbol: %-6d [%s]\n",(int)way->smbl, + p[way->smbl-x]); + (void) fprintf(outf,"\tAltitude: %d\n",(int)way->alt); + (void) fprintf(outf,"\tDepth: %f\n",way->dpth); + (void) fprintf(outf,"\tState: %-2.2s\n",way->state); + (void) fprintf(outf,"\tCountry: %-2.2s\n",way->cc); + (void) fprintf(outf,"\tClass: %d\n",way->wpt_class); + if(way->wpt_class>=0x80 && way->wpt_class<=0x85) + (void) fprintf(outf,"\tSubclass: %18.18s\n",way->subclass); + if(!way->wpt_class) + (void) fprintf(outf,"\tComment: %s\n",way->cmnt); + if(way->wpt_class>=0x40 && way->wpt_class<=0x46) + { + (void) fprintf(outf,"\tFacility: %s\n",way->facility); + (void) fprintf(outf,"\tCity: %s\n",way->city); + } + if(way->wpt_class==0x83) + (void) fprintf(outf,"\tAddress: %s\n",way->addr); + if(way->wpt_class==0x82) + (void) fprintf(outf,"\tCross Road: %s\n",way->cross_road); + + + return; +} + + +/* @funcstatic GPS_Fmt_Print_Way150 ************************************* +** +** Output waypoint D150 +** +** @param [r] way [GPS_PWay] waypoint +** @param [w] outf [FILE *] output stream +** +** @return [void] +************************************************************************/ + +static void GPS_Fmt_Print_Way150(GPS_PWay way, FILE *outf) +{ + + (void) fprintf(outf,"\tIdent: %-6.6s\n",way->ident); + (void) fprintf(outf,"\tLatitude: %f\n",way->lat); + (void) fprintf(outf,"\tLongitude: %f\n",way->lon); + (void) fprintf(outf,"\tComment: %-40.40s\n",way->cmnt); + (void) fprintf(outf,"\tClass: %d\n",way->wpt_class); + if(way->wpt_class!=4) + { + (void) fprintf(outf,"\tCountry: %-2.2s\n",way->cc); + (void) fprintf(outf,"\tCity: %-24.24s\n",way->city); + (void) fprintf(outf,"\tState: %-2.2s\n",way->state); + (void) fprintf(outf,"\tName: %-30.30s\n",way->name); + } + if(!way->wpt_class) + (void) fprintf(outf,"\tAltitude: %d\n",(int)way->alt); + + return; +} + + +/* @funcstatic GPS_Fmt_Print_Way151 ************************************ +** +** Output waypoint D151 +** +** @param [r] way [GPS_PWay] waypoint +** @param [w] outf [FILE *] output stream +** +** @return [void] +************************************************************************/ + +static void GPS_Fmt_Print_Way151(GPS_PWay way, FILE *outf) +{ + + (void) fprintf(outf,"\tIdent: %-6.6s\n",way->ident); + (void) fprintf(outf,"\tLatitude: %f\n",way->lat); + (void) fprintf(outf,"\tLongitude: %f\n",way->lon); + (void) fprintf(outf,"\tComment: %-40.40s\n",way->cmnt); + (void) fprintf(outf,"\tClass: %d\n",way->wpt_class); + if(way->wpt_class!=2) + { + (void) fprintf(outf,"\tCountry: %-2.2s\n",way->cc); + (void) fprintf(outf,"\tCity: %-24.24s\n",way->city); + (void) fprintf(outf,"\tState: %-2.2s\n",way->state); + (void) fprintf(outf,"\tName: %-30.30s\n",way->name); + } + if(!way->wpt_class) + (void) fprintf(outf,"\tAltitude: %d\n",(int)way->alt); + + return; +} + + + +/* @funcstatic GPS_Fmt_Print_Way152 ************************************ +** +** Output waypoint D152 +** +** @param [r] way [GPS_PWay] waypoint +** @param [w] outf [FILE *] output stream +** +** @return [void] +************************************************************************/ + +static void GPS_Fmt_Print_Way152(GPS_PWay way, FILE *outf) +{ + + (void) fprintf(outf,"\tIdent: %-6.6s\n",way->ident); + (void) fprintf(outf,"\tLatitude: %f\n",way->lat); + (void) fprintf(outf,"\tLongitude: %f\n",way->lon); + (void) fprintf(outf,"\tComment: %-40.40s\n",way->cmnt); + (void) fprintf(outf,"\tClass: %d\n",way->wpt_class); + if(way->wpt_class!=4) + { + (void) fprintf(outf,"\tCountry: %-2.2s\n",way->cc); + (void) fprintf(outf,"\tCity: %-24.24s\n",way->city); + (void) fprintf(outf,"\tState: %-2.2s\n",way->state); + (void) fprintf(outf,"\tName: %-30.30s\n",way->name); + } + if(!way->wpt_class) + (void) fprintf(outf,"\tAltitude: %d\n",(int)way->alt); + + return; +} + + +/* @funcstatic GPS_Fmt_Print_Way154 ************************************ +** +** Output waypoint D154 +** +** @param [r] way [GPS_PWay] waypoint +** @param [w] outf [FILE *] output stream +** +** @return [void] +************************************************************************/ + +static void GPS_Fmt_Print_Way154(GPS_PWay way, FILE *outf) +{ + char **p; + int32 x; + + if(way->smbl < 8192) + { + p = gps_marine_sym; + x = 0; + } + else if(way->smbl < 16384) + { + p = gps_land_sym; + x = 8192; + } + else + { + p = gps_aviation_sym; + x = 16384; + } + + + (void) fprintf(outf,"\tIdent: %-6.6s\n",way->ident); + (void) fprintf(outf,"\tLatitude: %f\n",way->lat); + (void) fprintf(outf,"\tLongitude: %f\n",way->lon); + (void) fprintf(outf,"\tComment: %-40.40s\n",way->cmnt); + (void) fprintf(outf,"\tSymbol: %-6d [%s]\n",(int)way->smbl, + p[way->smbl-x]); + (void) fprintf(outf,"\tClass: %d\n",way->wpt_class); + if(way->wpt_class!=4 && way->wpt_class!=8) + { + (void) fprintf(outf,"\tCountry: %-2.2s\n",way->cc); + (void) fprintf(outf,"\tCity: %-24.24s\n",way->city); + (void) fprintf(outf,"\tState: %-2.2s\n",way->state); + (void) fprintf(outf,"\tName: %-30.30s\n",way->name); + } + if(!way->wpt_class) + (void) fprintf(outf,"\tAltitude: %d\n",(int)way->alt); + + return; +} + + +/* @funcstatic GPS_Fmt_Print_Way155 ************************************* +** +** Output waypoint D155 +** +** @param [r] way [GPS_PWay] waypoint +** @param [w] outf [FILE *] output stream +** +** @return [void] +************************************************************************/ + +static void GPS_Fmt_Print_Way155(GPS_PWay way, FILE *outf) +{ + static char *dspl[]= + { + "","S","","SW","","SC" + }; + + char **p; + int32 x; + + if(way->smbl < 8192) + { + p = gps_marine_sym; + x = 0; + } + else if(way->smbl < 16384) + { + p = gps_land_sym; + x = 8192; + } + else + { + p = gps_aviation_sym; + x = 16384; + } + + + (void) fprintf(outf,"\tIdent: %-6.6s\n",way->ident); + (void) fprintf(outf,"\tLatitude: %f\n",way->lat); + (void) fprintf(outf,"\tLongitude: %f\n",way->lon); + (void) fprintf(outf,"\tComment: %-40.40s\n",way->cmnt); + (void) fprintf(outf,"\tSymbol: %-6d [%s]\n",(int)way->smbl, + p[way->smbl-x]); + (void) fprintf(outf,"\tDisplay: %-6d [%s]\n",(int)way->dspl, + dspl[way->dspl]); + (void) fprintf(outf,"\tClass: %d\n",way->wpt_class); + if(way->wpt_class!=4 && way->wpt_class!=8) + { + (void) fprintf(outf,"\tCountry: %-2.2s\n",way->cc); + (void) fprintf(outf,"\tCity: %-24.24s\n",way->city); + (void) fprintf(outf,"\tState: %-2.2s\n",way->state); + (void) fprintf(outf,"\tName: %-30.30s\n",way->name); + } + if(!way->wpt_class) + (void) fprintf(outf,"\tAltitude: %d\n",(int)way->alt); + + return; +} + + + +/* @func GPS_Fmt_Print_Route ***************************************** +** +** Output route(s) +** +** @param [r] way [GPS_PWay *] waypoint array +** @param [r] n [int32] number of waypoint entries +** @param [w] outf [FILE *] output stream +** +** @return [int32] success +************************************************************************/ + +int32 GPS_Fmt_Print_Route(GPS_PWay *way, int32 n, FILE *outf) +{ + int32 i; + int32 first; + + if(!n) + return 1; + + + switch(gps_route_transfer) + { + case pA200: + break; + case pA201: + return GPS_Fmt_Print_Route201(way,n,outf); + default: + GPS_Error("GPS_Fmt_Print_Route: Unknown protocol"); + return PROTOCOL_ERROR; + } + + + (void) fprintf(outf,"Route log 200 %d\n#\n",(int)gps_rte_type); + (void) fprintf(outf,"Start\n#\n"); + + + + first = 1; + + for(i=0;iisrte) + { + if(!first) + (void) fprintf(outf,"End\n#\n"); + (void) fprintf(outf,"Route Type: %d ",(int)way[i]->rte_prot); + first=0; + + switch(way[i]->rte_prot) + { + case 200: + (void) fprintf(outf,"Number: %d",way[i]->rte_num); + break; + case 201: + (void) fprintf(outf,"Number: %d Comment: %-20.20s", + way[i]->rte_num,way[i]->rte_cmnt); + break; + case 202: + (void) fprintf(outf,"Comment: %s",way[i]->rte_ident); + break; + default: + GPS_Error("Print_Route: Unknown route protocol"); + return PROTOCOL_ERROR; + } + (void) fprintf(outf,"\n#\n"); + (void) fprintf(outf,"Waypoints Type: %d\n#\nStart\n#\n" + ,(int)way[i]->prot); + continue; + } + + switch(way[i]->prot) + { + case 100: + GPS_Fmt_Print_Way100(way[i],outf); + break; + case 101: + GPS_Fmt_Print_Way101(way[i],outf); + break; + case 102: + GPS_Fmt_Print_Way102(way[i],outf); + break; + case 103: + GPS_Fmt_Print_Way103(way[i],outf); + break; + case 104: + GPS_Fmt_Print_Way104(way[i],outf); + break; + case 105: + GPS_Fmt_Print_Way105(way[i],outf); + break; + case 106: + GPS_Fmt_Print_Way106(way[i],outf); + break; + case 107: + GPS_Fmt_Print_Way107(way[i],outf); + break; + case 108: + GPS_Fmt_Print_Way108(way[i],outf); + break; + case 150: + GPS_Fmt_Print_Way150(way[i],outf); + break; + case 151: + GPS_Fmt_Print_Way151(way[i],outf); + break; + case 152: + GPS_Fmt_Print_Way152(way[i],outf); + break; + case 154: + GPS_Fmt_Print_Way154(way[i],outf); + break; + case 155: + GPS_Fmt_Print_Way155(way[i],outf); + break; + default: + GPS_Error("Print_Route: Unknown waypoint protocol"); + return PROTOCOL_ERROR; + } + (void) fprintf(outf,"#\n"); + } + (void) fprintf(outf,"End\n#\n"); + + return 1; +} + + + +/* @funcstatic GPS_Fmt_Print_Route201 *********************************** +** +** Output route(s) +** +** @param [r] way [GPS_PWay *] waypoint array +** @param [r] n [int32] number of waypoint entries +** @param [w] outf [FILE *] output stream +** +** @return [int32] success +************************************************************************/ + +static int32 GPS_Fmt_Print_Route201(GPS_PWay *way, int32 n, FILE *outf) +{ + int32 i; + int32 first; + + if(!n) + return 1; + + + (void) fprintf(outf,"Route log 201 %d\n#\n",(int)gps_rte_link_type); + (void) fprintf(outf,"Start\n#\n"); + + + first = 1; + + for(i=0;iisrte) + { + if(!first) + (void) fprintf(outf,"End\n#\n"); + (void) fprintf(outf,"Route Type: %d ",(int)way[i]->rte_prot); + first=0; + + switch(way[i]->rte_prot) + { + case 200: + (void) fprintf(outf,"Number: %d",way[i]->rte_num); + break; + case 201: + (void) fprintf(outf,"Number: %d Comment: %-20.20s", + way[i]->rte_num,way[i]->rte_cmnt); + break; + case 202: + (void) fprintf(outf,"Comment: %s",way[i]->rte_ident); + break; + default: + GPS_Error("Print_Route: Unknown route protocol"); + return PROTOCOL_ERROR; + } + (void) fprintf(outf,"\n#\n"); + (void) fprintf(outf,"Waypoints Type: %d\n#\n" + ,(int)way[i]->prot); + continue; + } + + + if(way[i]->islink) + { + (void) fprintf(outf,"\tLink Class: %d\n", + (int)way[i]->rte_link_class); + if(!(way[i]->rte_link_class==3 || way[i]->rte_link_class==0xff)) + (void) fprintf(outf,"\tLink Subclass: %-18.18s\n", + way[i]->rte_link_subclass); + (void) fprintf(outf,"\tLink Ident: %s\n#\n", + way[i]->rte_link_ident); + continue; + } + + + switch(way[i]->prot) + { + case 100: + GPS_Fmt_Print_Way100(way[i],outf); + break; + case 101: + GPS_Fmt_Print_Way101(way[i],outf); + break; + case 102: + GPS_Fmt_Print_Way102(way[i],outf); + break; + case 103: + GPS_Fmt_Print_Way103(way[i],outf); + break; + case 104: + GPS_Fmt_Print_Way104(way[i],outf); + break; + case 105: + GPS_Fmt_Print_Way105(way[i],outf); + break; + case 106: + GPS_Fmt_Print_Way106(way[i],outf); + break; + case 107: + GPS_Fmt_Print_Way107(way[i],outf); + break; + case 108: + GPS_Fmt_Print_Way108(way[i],outf); + break; + case 150: + GPS_Fmt_Print_Way150(way[i],outf); + break; + case 151: + GPS_Fmt_Print_Way151(way[i],outf); + break; + case 152: + GPS_Fmt_Print_Way152(way[i],outf); + break; + case 154: + GPS_Fmt_Print_Way154(way[i],outf); + break; + case 155: + GPS_Fmt_Print_Way155(way[i],outf); + break; + default: + GPS_Error("Print_Route: Unknown waypoint protocol"); + return PROTOCOL_ERROR; + } + (void) fprintf(outf,"#\n"); + } + (void) fprintf(outf,"End\n"); + + return 1; +} diff --git a/gpsbabel/jeeps/gpsfmt.h b/gpsbabel/jeeps/gpsfmt.h new file mode 100644 index 000000000..f9eaae32c --- /dev/null +++ b/gpsbabel/jeeps/gpsfmt.h @@ -0,0 +1,27 @@ +#ifdef __cplusplus +extern "C" +{ +#endif + +#ifndef gpsfmt_h +#define gpsfmt_h + + +#include "gps.h" +#include +#include + +void GPS_Fmt_Print_Time(time_t Time, FILE *outf); +void GPS_Fmt_Print_Position(double lat, double lon, FILE *outf); +void GPS_Fmt_Print_Pvt(GPS_PPvt_Data pvt, FILE *outf); +void GPS_Fmt_Print_Almanac(GPS_PAlmanac *alm, int32 n, FILE *outf); +void GPS_Fmt_Print_Track(GPS_PTrack *trk, int32 n, FILE *outf); +int32 GPS_Fmt_Print_Waypoint(GPS_PWay *way, int32 n, FILE *outf); +int32 GPS_Fmt_Print_Proximity(GPS_PWay *way, int32 n, FILE *outf); +int32 GPS_Fmt_Print_Route(GPS_PWay *way, int32 n, FILE *outf); + +#endif + +#ifdef __cplusplus +} +#endif diff --git a/gpsbabel/jeeps/gpsinput.c b/gpsbabel/jeeps/gpsinput.c new file mode 100644 index 000000000..30af011b4 --- /dev/null +++ b/gpsbabel/jeeps/gpsinput.c @@ -0,0 +1,2107 @@ +/******************************************************************** +** @source JEEPS input functions +** +** @author Copyright (C) 1999 Alan Bleasby +** @version 1.0 +** @modified Dec 28 1999 Alan Bleasby. First version +** @@ +** +** This library is free software; you can redistribute it and/or +** modify it under the terms of the GNU Library General Public +** License as published by the Free Software Foundation; either +** version 2 of the License, or (at your option) any later version. +** +** This library is distributed in the hope that it will be useful, +** but WITHOUT ANY WARRANTY; without even the implied warranty of +** MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU +** Library General Public License for more details. +** +** You should have received a copy of the GNU Library General Public +** License along with this library; if not, write to the +** Free Software Foundation, Inc., 59 Temple Place - Suite 330, +** Boston, MA 02111-1307, USA. +********************************************************************/ +#include "gps.h" +#include +#include +#include +#include + + +static int32 GPS_Input_Load_String(char *t, int32 n, char *s); +static int32 GPS_Input_Load_Strnull(char *t, char *s); +static int32 GPS_Input_Read_Line(char *s, FILE *inf); + +static int32 GPS_Input_Get_D100(GPS_PWay *way, FILE *inf); +static int32 GPS_Input_Get_D101(GPS_PWay *way, FILE *inf); +static int32 GPS_Input_Get_D102(GPS_PWay *way, FILE *inf); +static int32 GPS_Input_Get_D103(GPS_PWay *way, FILE *inf); +static int32 GPS_Input_Get_D104(GPS_PWay *way, FILE *inf); +static int32 GPS_Input_Get_D105(GPS_PWay *way, FILE *inf); +static int32 GPS_Input_Get_D106(GPS_PWay *way, FILE *inf); +static int32 GPS_Input_Get_D107(GPS_PWay *way, FILE *inf); +static int32 GPS_Input_Get_D108(GPS_PWay *way, FILE *inf); +static int32 GPS_Input_Get_D150(GPS_PWay *way, FILE *inf); +static int32 GPS_Input_Get_D151(GPS_PWay *way, FILE *inf); +static int32 GPS_Input_Get_D152(GPS_PWay *way, FILE *inf); +static int32 GPS_Input_Get_D154(GPS_PWay *way, FILE *inf); +static int32 GPS_Input_Get_D155(GPS_PWay *way, FILE *inf); + +static int32 GPS_Input_Get_Track301(GPS_PTrack **trk, FILE *inf, int32 type, + int32 n); +static int32 GPS_Input_Get_D300(GPS_PTrack *trk, FILE *inf, char *s); +static int32 GPS_Input_Get_D301(GPS_PTrack *trk, FILE *inf, char *s); + +static int32 GPS_Input_Get_Route201(GPS_PWay **way, FILE *inf); + + +/* @funcstatic GPS_Input_Load_String *********************************** +** +** Load a GPS char type from an input line +** Remove trailing newline +** +** @param [w] t [char *] string to load +** @param [r] n [int32] maximum type length +** @param [r] s [char *] source line +** +** @return [int32] success +************************************************************************/ +static int32 GPS_Input_Load_String(char *t, int32 n, char *s) +{ + char *p; + char *q; + + int32 len; + int32 i; + + gps_errno = INPUT_ERROR; + + p=s; + if(!(p=strchr(p,':'))) + return gps_errno; + ++p; + while(*p && (*p==' ' || *p=='\t')) ++p; + if(!*p) + return 0; + + len = strlen(p); + q = p+len-1; + while(*q==' ' || *q=='\t') --q; + len = q-p+1; + + if(q-p+1 > n) + { + len = n; + p[n]='\0'; + } + for(i=0;isvid = i; + (*alm)[i]->wn = -1; + } + } + else + { + if(!(*alm = (GPS_PAlmanac *) malloc(n*sizeof(GPS_PAlmanac *)))) + return MEMORY_ERROR; + for(i=0;i<32;++i) + if(!((*alm)[i] = GPS_Almanac_New())) + return MEMORY_ERROR; + } + + for(i=0;isvid!=d) ++i; + (*alm)[i]->svid=d; + + if(!GPS_Input_Read_Line(s,inf)) + return gps_errno; + p=strchr(s,':'); + if(sscanf(p+1,"%d",(int *)&d)!=1) + return gps_errno; + (*alm)[i]->wn = d; + + if(!GPS_Input_Read_Line(s,inf)) + return gps_errno; + p=strchr(s,':'); + if(sscanf(p+1,"%f",&f)!=1) + return gps_errno; + (*alm)[i]->toa = f; + + if(!GPS_Input_Read_Line(s,inf)) + return gps_errno; + p=strchr(s,':'); + if(sscanf(p+1,"%f",&f)!=1) + return gps_errno; + (*alm)[i]->af0 = f; + + if(!GPS_Input_Read_Line(s,inf)) + return gps_errno; + p=strchr(s,':'); + if(sscanf(p+1,"%f",&f)!=1) + return gps_errno; + (*alm)[i]->af1 = f; + + if(!GPS_Input_Read_Line(s,inf)) + return gps_errno; + p=strchr(s,':'); + if(sscanf(p+1,"%f",&f)!=1) + return gps_errno; + (*alm)[i]->e = f; + + if(!GPS_Input_Read_Line(s,inf)) + return gps_errno; + p=strchr(s,':'); + if(sscanf(p+1,"%f",&f)!=1) + return gps_errno; + (*alm)[i]->sqrta = f; + + if(!GPS_Input_Read_Line(s,inf)) + return gps_errno; + p=strchr(s,':'); + if(sscanf(p+1,"%f",&f)!=1) + return gps_errno; + (*alm)[i]->m0 = f; + + if(!GPS_Input_Read_Line(s,inf)) + return gps_errno; + p=strchr(s,':'); + if(sscanf(p+1,"%f",&f)!=1) + return gps_errno; + (*alm)[i]->w = f; + + if(!GPS_Input_Read_Line(s,inf)) + return gps_errno; + p=strchr(s,':'); + if(sscanf(p+1,"%f",&f)!=1) + return gps_errno; + (*alm)[i]->omg0 = f; + + if(!GPS_Input_Read_Line(s,inf)) + return gps_errno; + p=strchr(s,':'); + if(sscanf(p+1,"%f",&f)!=1) + return gps_errno; + (*alm)[i]->odot = f; + + if(!GPS_Input_Read_Line(s,inf)) + return gps_errno; + p=strchr(s,':'); + if(sscanf(p+1,"%f",&f)!=1) + return gps_errno; + (*alm)[i]->i = f; + + if(!GPS_Input_Read_Line(s,inf)) + return gps_errno; + p=strchr(s,':'); + if(sscanf(p+1,"%d",(int *)&d)!=1) + return gps_errno; + (*alm)[i]->hlth=d; + } + + if(!type) + n = 32; + + return n; +} + + + +/* @func GPS_Input_Get_Waypoint ***************************************** +** +** Construct a waypoint array from a file +** +** @param [w] way [GPS_PWay **] pointer to waypoint array +** @param [r] inf [FILE *] stream +** +** @return [int32] number of entries +************************************************************************/ + +int32 GPS_Input_Get_Waypoint(GPS_PWay **way, FILE *inf) +{ + char s[GPS_ARB_LEN]; + int32 n; + int32 type; + int32 i; + long pos; + int32 ret; + + gps_errno = INPUT_ERROR; + + if(!GPS_Input_Read_Line(s,inf)) + return gps_errno; + if(sscanf(s,"Waypoints Type: %d",(int *)&type)!=1) + return gps_errno; + + if(!GPS_Input_Read_Line(s,inf)) + return gps_errno; + if(strncmp(s,"Start",5)) + return gps_errno; + + pos = ftell(inf); + n = 0; + while(strncmp(s,"End",3)) + { + if(!GPS_Input_Read_Line(s,inf)) + return gps_errno; + if(strstr(s,"Latitude")) + ++n; + } + fseek(inf,pos,0); + + if(!(*way=(GPS_PWay *)malloc(n*sizeof(GPS_PWay *)))) + return MEMORY_ERROR; + for(i=0;iprot = type; + } + + + for(i=0;iprot = type; + } + + for(i=0;idst = f; + } + + return n; +} + + + +/* @funcstatic GPS_Input_Get_D100 ************************************ +** +** Get a D100 Entry +** +** @param [w] way [GPS_PWay *] pointer to waypoint entry +** @param [r] inf [FILE *] stream +** +** @return [int32] number of entries +************************************************************************/ +static int32 GPS_Input_Get_D100(GPS_PWay *way, FILE *inf) +{ + char s[GPS_ARB_LEN]; + char *p; + + double f; + + if(!GPS_Input_Read_Line(s,inf)) + return gps_errno; + GPS_Input_Load_String((*way)->ident,6,s); + + if(!GPS_Input_Read_Line(s,inf)) + return gps_errno; + p=strchr(s,':'); + if(sscanf(p+1,"%lf",&f)!=1) + return gps_errno; + (*way)->lat = f; + + if(!GPS_Input_Read_Line(s,inf)) + return gps_errno; + p=strchr(s,':'); + if(sscanf(p+1,"%lf",&f)!=1) + return gps_errno; + (*way)->lon = f; + + if(!GPS_Input_Read_Line(s,inf)) + return gps_errno; + GPS_Input_Load_String((*way)->cmnt,40,s); + + return 1; +} + + + +/* @funcstatic GPS_Input_Get_D101 ************************************ +** +** Get a D101 Entry +** +** @param [w] way [GPS_PWay *] pointer to waypoint entry +** @param [r] inf [FILE *] stream +** +** @return [int32] number of entries +************************************************************************/ +static int32 GPS_Input_Get_D101(GPS_PWay *way, FILE *inf) +{ + char s[GPS_ARB_LEN]; + char *p; + + double f; + int32 d; + + + if(!GPS_Input_Read_Line(s,inf)) + return gps_errno; + GPS_Input_Load_String((*way)->ident,6,s); + + if(!GPS_Input_Read_Line(s,inf)) + return gps_errno; + p=strchr(s,':'); + if(sscanf(p+1,"%lf",&f)!=1) + return gps_errno; + (*way)->lat = f; + + if(!GPS_Input_Read_Line(s,inf)) + return gps_errno; + p=strchr(s,':'); + if(sscanf(p+1,"%lf",&f)!=1) + return gps_errno; + (*way)->lon = f; + + if(!GPS_Input_Read_Line(s,inf)) + return gps_errno; + GPS_Input_Load_String((*way)->cmnt,40,s); + + if(!GPS_Input_Read_Line(s,inf)) + return gps_errno; + p=strchr(s,':'); + if(sscanf(p+1,"%d",(int *)&d)!=1) + return gps_errno; + (*way)->smbl = d; + + return 1; +} + + + +/* @funcstatic GPS_Input_Get_D102 ************************************ +** +** Get a D102 Entry +** +** @param [w] way [GPS_PWay *] pointer to waypoint entry +** @param [r] inf [FILE *] stream +** +** @return [int32] number of entries +************************************************************************/ +static int32 GPS_Input_Get_D102(GPS_PWay *way, FILE *inf) +{ + return GPS_Input_Get_D101(way,inf); +} + + + +/* @funcstatic GPS_Input_Get_D103 ************************************ +** +** Get a D103 Entry +** +** @param [w] way [GPS_PWay *] pointer to waypoint entry +** @param [r] inf [FILE *] stream +** +** @return [int32] number of entries +************************************************************************/ +static int32 GPS_Input_Get_D103(GPS_PWay *way, FILE *inf) +{ + char s[GPS_ARB_LEN]; + char *p; + + double f; + int32 d; + + + if(!GPS_Input_Read_Line(s,inf)) + return gps_errno; + GPS_Input_Load_String((*way)->ident,6,s); + + if(!GPS_Input_Read_Line(s,inf)) + return gps_errno; + p=strchr(s,':'); + if(sscanf(p+1,"%lf",&f)!=1) + return gps_errno; + (*way)->lat = f; + + if(!GPS_Input_Read_Line(s,inf)) + return gps_errno; + p=strchr(s,':'); + if(sscanf(p+1,"%lf",&f)!=1) + return gps_errno; + (*way)->lon = f; + + if(!GPS_Input_Read_Line(s,inf)) + return gps_errno; + GPS_Input_Load_String((*way)->cmnt,40,s); + + if(!GPS_Input_Read_Line(s,inf)) + return gps_errno; + p=strchr(s,':'); + if(sscanf(p+1,"%d",(int *)&d)!=1) + return gps_errno; + (*way)->smbl = d; + + if(!GPS_Input_Read_Line(s,inf)) + return gps_errno; + p=strchr(s,':'); + if(sscanf(p+1,"%d",(int *)&d)!=1) + return gps_errno; + (*way)->dspl = d; + + return 1; +} + + + +/* @funcstatic GPS_Input_Get_D104 ************************************ +** +** Get a D104 Entry +** +** @param [w] way [GPS_PWay *] pointer to waypoint entry +** @param [r] inf [FILE *] stream +** +** @return [int32] number of entries +************************************************************************/ +static int32 GPS_Input_Get_D104(GPS_PWay *way, FILE *inf) +{ + return GPS_Input_Get_D103(way,inf); +} + + + +/* @funcstatic GPS_Input_Get_D105 ************************************ +** +** Get a D105 Entry +** +** @param [w] way [GPS_PWay *] pointer to waypoint entry +** @param [r] inf [FILE *] stream +** +** @return [int32] number of entries +************************************************************************/ +static int32 GPS_Input_Get_D105(GPS_PWay *way, FILE *inf) +{ + char s[GPS_ARB_LEN]; + char *p; + + double f; + int32 d; + + + if(!GPS_Input_Read_Line(s,inf)) + return gps_errno; + p=strchr(s,':'); + if(sscanf(p+1,"%lf",&f)!=1) + return gps_errno; + (*way)->lat = f; + + if(!GPS_Input_Read_Line(s,inf)) + return gps_errno; + p=strchr(s,':'); + if(sscanf(p+1,"%lf",&f)!=1) + return gps_errno; + (*way)->lon = f; + + if(!GPS_Input_Read_Line(s,inf)) + return gps_errno; + p=strchr(s,':'); + if(sscanf(p+1,"%d",(int *)&d)!=1) + return gps_errno; + (*way)->smbl = d; + + if(!GPS_Input_Read_Line(s,inf)) + return gps_errno; + GPS_Input_Load_Strnull((*way)->wpt_ident,s); + + return 1; +} + + + +/* @funcstatic GPS_Input_Get_D106 ************************************ +** +** Get a D106 Entry +** +** @param [w] way [GPS_PWay *] pointer to waypoint entry +** @param [r] inf [FILE *] stream +** +** @return [int32] number of entries +************************************************************************/ +static int32 GPS_Input_Get_D106(GPS_PWay *way, FILE *inf) +{ + char s[GPS_ARB_LEN]; + char *p; + + double f; + int32 d; + + if(!GPS_Input_Read_Line(s,inf)) + return gps_errno; + p=strchr(s,':'); + if(sscanf(p+1,"%d",(int *)&d)!=1) + return gps_errno; + (*way)->wpt_class = d; + + if(!GPS_Input_Read_Line(s,inf)) + return gps_errno; + GPS_Input_Load_String((char *)(*way)->subclass,13,s); + + + if(!GPS_Input_Read_Line(s,inf)) + return gps_errno; + GPS_Input_Load_String((*way)->ident,6,s); + + if(!GPS_Input_Read_Line(s,inf)) + return gps_errno; + p=strchr(s,':'); + if(sscanf(p+1,"%lf",&f)!=1) + return gps_errno; + (*way)->lat = f; + + if(!GPS_Input_Read_Line(s,inf)) + return gps_errno; + p=strchr(s,':'); + if(sscanf(p+1,"%lf",&f)!=1) + return gps_errno; + (*way)->lon = f; + + if(!GPS_Input_Read_Line(s,inf)) + return gps_errno; + p=strchr(s,':'); + if(sscanf(p+1,"%d",(int *)&d)!=1) + return gps_errno; + (*way)->smbl = d; + + if(!GPS_Input_Read_Line(s,inf)) + return gps_errno; + GPS_Input_Load_Strnull((*way)->wpt_ident,s); + + if(!GPS_Input_Read_Line(s,inf)) + return gps_errno; + GPS_Input_Load_Strnull((*way)->lnk_ident,s); + + return 1; +} + + + +/* @funcstatic GPS_Input_Get_D107 ************************************ +** +** Get a D107 Entry +** +** @param [w] way [GPS_PWay *] pointer to waypoint entry +** @param [r] inf [FILE *] stream +** +** @return [int32] number of entries +************************************************************************/ +static int32 GPS_Input_Get_D107(GPS_PWay *way, FILE *inf) +{ + char s[GPS_ARB_LEN]; + char *p; + + int32 d; + int32 ret; + + if((ret=GPS_Input_Get_D103(way,inf))<0) + return ret; + + if(!GPS_Input_Read_Line(s,inf)) + return gps_errno; + p=strchr(s,':'); + if(sscanf(p+1,"%d",(int *)&d)!=1) + return gps_errno; + (*way)->colour = d; + + return 1; +} + + + +/* @funcstatic GPS_Input_Get_D108 ************************************ +** +** Get a D108 Entry +** +** @param [w] way [GPS_PWay *] pointer to waypoint entry +** @param [r] inf [FILE *] stream +** +** @return [int32] number of entries +************************************************************************/ +static int32 GPS_Input_Get_D108(GPS_PWay *way, FILE *inf) +{ + char s[GPS_ARB_LEN]; + char *p; + double f; + int32 d; + int32 xc; + + + if(!GPS_Input_Read_Line(s,inf)) + return gps_errno; + GPS_Input_Load_Strnull((*way)->ident,s); + + if(!GPS_Input_Read_Line(s,inf)) + return gps_errno; + p=strchr(s,':'); + if(sscanf(p+1,"%lf",&f)!=1) + return gps_errno; + (*way)->lat = f; + + if(!GPS_Input_Read_Line(s,inf)) + return gps_errno; + p=strchr(s,':'); + if(sscanf(p+1,"%lf",&f)!=1) + return gps_errno; + (*way)->lon = f; + + if(!GPS_Input_Read_Line(s,inf)) + return gps_errno; + p=strchr(s,':'); + if(sscanf(p+1,"%d",(int *)&d)!=1) + return gps_errno; + (*way)->colour = d; + + if(!GPS_Input_Read_Line(s,inf)) + return gps_errno; + p=strchr(s,':'); + if(sscanf(p+1,"%d",(int *)&d)!=1) + return gps_errno; + (*way)->dspl = d; + + if(!GPS_Input_Read_Line(s,inf)) + return gps_errno; + p=strchr(s,':'); + if(sscanf(p+1,"%d",(int *)&d)!=1) + return gps_errno; + (*way)->smbl = d; + + if(!GPS_Input_Read_Line(s,inf)) + return gps_errno; + p=strchr(s,':'); + if(sscanf(p+1,"%d",(int *)&d)!=1) + return gps_errno; + (*way)->alt = d; + + if(!GPS_Input_Read_Line(s,inf)) + return gps_errno; + p=strchr(s,':'); + if(sscanf(p+1,"%lf",&f)!=1) + return gps_errno; + (*way)->dpth = f; + + if(!GPS_Input_Read_Line(s,inf)) + return gps_errno; + GPS_Input_Load_String((*way)->state,2,s); + + if(!GPS_Input_Read_Line(s,inf)) + return gps_errno; + GPS_Input_Load_String((*way)->cc,2,s); + + if(!GPS_Input_Read_Line(s,inf)) + return gps_errno; + p=strchr(s,':'); + if(sscanf(p+1,"%d",(int *)&d)!=1) + return gps_errno; + (*way)->wpt_class = d; + xc = d; + + if(xc>=0x80 && xc<=0x85) + { + if(!GPS_Input_Read_Line(s,inf)) + return gps_errno; + GPS_Input_Load_String((char *)(*way)->subclass,18,s); + } + else + { + GPS_Util_Put_Short((*way)->subclass,0); + GPS_Util_Put_Int((*way)->subclass+2,0); + GPS_Util_Put_Uint((*way)->subclass+6,0xffffffff); + GPS_Util_Put_Uint((*way)->subclass+10,0xffffffff); + GPS_Util_Put_Uint((*way)->subclass+14,0xffffffff); + } + + if(!xc) + { + if(!GPS_Input_Read_Line(s,inf)) + return gps_errno; + GPS_Input_Load_Strnull((*way)->cmnt,s); + } + + if(xc>=0x40 && xc<=0x46) + { + if(!GPS_Input_Read_Line(s,inf)) + return gps_errno; + GPS_Input_Load_Strnull((*way)->facility,s); + if(!GPS_Input_Read_Line(s,inf)) + return gps_errno; + GPS_Input_Load_Strnull((*way)->city,s); + } + + if(xc==0x83) + { + if(!GPS_Input_Read_Line(s,inf)) + return gps_errno; + GPS_Input_Load_Strnull((*way)->addr,s); + } + + if(xc==0x82) + { + if(!GPS_Input_Read_Line(s,inf)) + return gps_errno; + GPS_Input_Load_Strnull((*way)->cross_road,s); + } + + return 1; +} + + + +/* @funcstatic GPS_Input_Get_D150 ************************************ +** +** Get a D150 Entry +** +** @param [w] way [GPS_PWay *] pointer to waypoint entry +** @param [r] inf [FILE *] stream +** +** @return [int32] number of entries +************************************************************************/ +static int32 GPS_Input_Get_D150(GPS_PWay *way, FILE *inf) +{ + char s[GPS_ARB_LEN]; + char *p; + + double f; + int32 d; + int32 cl=0; + + + if(!GPS_Input_Read_Line(s,inf)) + return gps_errno; + GPS_Input_Load_String((*way)->ident,6,s); + + if(!GPS_Input_Read_Line(s,inf)) + return gps_errno; + p=strchr(s,':'); + if(sscanf(p+1,"%lf",&f)!=1) + return gps_errno; + (*way)->lat = f; + + if(!GPS_Input_Read_Line(s,inf)) + return gps_errno; + p=strchr(s,':'); + if(sscanf(p+1,"%lf",&f)!=1) + return gps_errno; + (*way)->lon = f; + + if(!GPS_Input_Read_Line(s,inf)) + return gps_errno; + GPS_Input_Load_String((*way)->cmnt,40,s); + + if(!GPS_Input_Read_Line(s,inf)) + return gps_errno; + p=strchr(s,':'); + if(sscanf(p+1,"%d",(int *)&d)!=1) + return gps_errno; + (*way)->wpt_class = cl = d; + + if(cl != 4) + { + if(!GPS_Input_Read_Line(s,inf)) + return gps_errno; + GPS_Input_Load_String((*way)->cc,2,s); + + if(!GPS_Input_Read_Line(s,inf)) + return gps_errno; + GPS_Input_Load_String((*way)->city,24,s); + + if(!GPS_Input_Read_Line(s,inf)) + return gps_errno; + GPS_Input_Load_String((*way)->state,2,s); + + if(!GPS_Input_Read_Line(s,inf)) + return gps_errno; + GPS_Input_Load_String((*way)->name,30,s); + } + + if(!cl) + { + if(!GPS_Input_Read_Line(s,inf)) + return gps_errno; + p=strchr(s,':'); + if(sscanf(p+1,"%d",(int *)&d)!=1) + return gps_errno; + (*way)->alt = d; + } + + return 1; +} + + + +/* @funcstatic GPS_Input_Get_D151 ************************************ +** +** Get a D151 Entry +** +** @param [w] way [GPS_PWay *] pointer to waypoint entry +** @param [r] inf [FILE *] stream +** +** @return [int32] number of entries +************************************************************************/ +static int32 GPS_Input_Get_D151(GPS_PWay *way, FILE *inf) +{ + char s[GPS_ARB_LEN]; + char *p; + + double f; + int32 d; + int32 cl=0; + + + if(!GPS_Input_Read_Line(s,inf)) + return gps_errno; + GPS_Input_Load_String((*way)->ident,6,s); + + if(!GPS_Input_Read_Line(s,inf)) + return gps_errno; + p=strchr(s,':'); + if(sscanf(p+1,"%lf",&f)!=1) + return gps_errno; + (*way)->lat = f; + + if(!GPS_Input_Read_Line(s,inf)) + return gps_errno; + p=strchr(s,':'); + if(sscanf(p+1,"%lf",&f)!=1) + return gps_errno; + (*way)->lon = f; + + if(!GPS_Input_Read_Line(s,inf)) + return gps_errno; + GPS_Input_Load_String((*way)->cmnt,40,s); + + if(!GPS_Input_Read_Line(s,inf)) + return gps_errno; + p=strchr(s,':'); + if(sscanf(p+1,"%d",(int *)&d)!=1) + return gps_errno; + (*way)->wpt_class = cl = d; + + if(cl != 2) + { + if(!GPS_Input_Read_Line(s,inf)) + return gps_errno; + GPS_Input_Load_String((*way)->cc,2,s); + + if(!GPS_Input_Read_Line(s,inf)) + return gps_errno; + GPS_Input_Load_String((*way)->city,24,s); + + if(!GPS_Input_Read_Line(s,inf)) + return gps_errno; + GPS_Input_Load_String((*way)->state,2,s); + + if(!GPS_Input_Read_Line(s,inf)) + return gps_errno; + GPS_Input_Load_String((*way)->name,30,s); + } + + if(!cl) + { + if(!GPS_Input_Read_Line(s,inf)) + return gps_errno; + p=strchr(s,':'); + if(sscanf(p+1,"%d",(int *)&d)!=1) + return gps_errno; + (*way)->alt = d; + } + + return 1; +} + + +/* @funcstatic GPS_Input_Get_D152 ************************************ +** +** Get a D152 Entry +** +** @param [w] way [GPS_PWay *] pointer to waypoint entry +** @param [r] inf [FILE *] stream +** +** @return [int32] number of entries +************************************************************************/ +static int32 GPS_Input_Get_D152(GPS_PWay *way, FILE *inf) +{ + return GPS_Input_Get_D150(way,inf); +} + + + +/* @funcstatic GPS_Input_Get_D154 ************************************ +** +** Get a D154 Entry +** +** @param [w] way [GPS_PWay *] pointer to waypoint entry +** @param [r] inf [FILE *] stream +** +** @return [int32] number of entries +************************************************************************/ +static int32 GPS_Input_Get_D154(GPS_PWay *way, FILE *inf) +{ + char s[GPS_ARB_LEN]; + char *p; + + double f; + int32 d; + int32 cl=0; + + + if(!GPS_Input_Read_Line(s,inf)) + return gps_errno; + GPS_Input_Load_String((*way)->ident,6,s); + + if(!GPS_Input_Read_Line(s,inf)) + return gps_errno; + p=strchr(s,':'); + if(sscanf(p+1,"%lf",&f)!=1) + return gps_errno; + (*way)->lat = f; + + if(!GPS_Input_Read_Line(s,inf)) + return gps_errno; + p=strchr(s,':'); + if(sscanf(p+1,"%lf",&f)!=1) + return gps_errno; + (*way)->lon = f; + + if(!GPS_Input_Read_Line(s,inf)) + return gps_errno; + GPS_Input_Load_String((*way)->cmnt,40,s); + + if(!GPS_Input_Read_Line(s,inf)) + return gps_errno; + p=strchr(s,':'); + if(sscanf(p+1,"%d",(int *)&d)!=1) + return gps_errno; + (*way)->smbl = d; + + if(!GPS_Input_Read_Line(s,inf)) + return gps_errno; + p=strchr(s,':'); + if(sscanf(p+1,"%d",(int *)&d)!=1) + return gps_errno; + (*way)->wpt_class = cl = d; + + if(cl != 4 && cl != 8) + { + if(!GPS_Input_Read_Line(s,inf)) + return gps_errno; + GPS_Input_Load_String((*way)->cc,2,s); + + if(!GPS_Input_Read_Line(s,inf)) + return gps_errno; + GPS_Input_Load_String((*way)->city,24,s); + + if(!GPS_Input_Read_Line(s,inf)) + return gps_errno; + GPS_Input_Load_String((*way)->state,2,s); + + if(!GPS_Input_Read_Line(s,inf)) + return gps_errno; + GPS_Input_Load_String((*way)->name,30,s); + } + + if(!cl) + { + if(!GPS_Input_Read_Line(s,inf)) + return gps_errno; + p=strchr(s,':'); + if(sscanf(p+1,"%d",(int *)&d)!=1) + return gps_errno; + (*way)->alt = d; + } + + return 1; +} + + + +/* @funcstatic GPS_Input_Get_D155 ************************************ +** +** Get a D155 Entry +** +** @param [w] way [GPS_PWay *] pointer to waypoint entry +** @param [r] inf [FILE *] stream +** +** @return [int32] number of entries +************************************************************************/ +static int32 GPS_Input_Get_D155(GPS_PWay *way, FILE *inf) +{ + char s[GPS_ARB_LEN]; + char *p; + + double f; + int32 d; + int32 cl=0; + + + if(!GPS_Input_Read_Line(s,inf)) + return gps_errno; + GPS_Input_Load_String((*way)->ident,6,s); + + if(!GPS_Input_Read_Line(s,inf)) + return gps_errno; + p=strchr(s,':'); + if(sscanf(p+1,"%lf",&f)!=1) + return gps_errno; + (*way)->lat = f; + + if(!GPS_Input_Read_Line(s,inf)) + return gps_errno; + p=strchr(s,':'); + if(sscanf(p+1,"%lf",&f)!=1) + return gps_errno; + (*way)->lon = f; + + if(!GPS_Input_Read_Line(s,inf)) + return gps_errno; + GPS_Input_Load_String((*way)->cmnt,40,s); + + if(!GPS_Input_Read_Line(s,inf)) + return gps_errno; + p=strchr(s,':'); + if(sscanf(p+1,"%d",(int *)&d)!=1) + return gps_errno; + (*way)->smbl = d; + + if(!GPS_Input_Read_Line(s,inf)) + return gps_errno; + p=strchr(s,':'); + if(sscanf(p+1,"%d",(int *)&d)!=1) + return gps_errno; + (*way)->dspl = d; + + if(!GPS_Input_Read_Line(s,inf)) + return gps_errno; + p=strchr(s,':'); + if(sscanf(p+1,"%d",(int *)&d)!=1) + return gps_errno; + (*way)->wpt_class = cl = d; + + if(cl != 4 && cl != 8) + { + if(!GPS_Input_Read_Line(s,inf)) + return gps_errno; + GPS_Input_Load_String((*way)->cc,2,s); + + if(!GPS_Input_Read_Line(s,inf)) + return gps_errno; + GPS_Input_Load_String((*way)->city,24,s); + + if(!GPS_Input_Read_Line(s,inf)) + return gps_errno; + GPS_Input_Load_String((*way)->state,2,s); + + if(!GPS_Input_Read_Line(s,inf)) + return gps_errno; + GPS_Input_Load_String((*way)->name,30,s); + } + + if(!cl) + { + if(!GPS_Input_Read_Line(s,inf)) + return gps_errno; + p=strchr(s,':'); + if(sscanf(p+1,"%d",(int *)&d)!=1) + return gps_errno; + (*way)->alt = d; + } + + return 1; +} + + + +/* @func GPS_Input_Get_Track ******************************************* +** +** Construct a travk array from a file +** +** @param [w] trk [GPS_PTrack **] pointer to track array +** @param [r] inf [FILE *] stream +** +** @return [int32] number of entries +************************************************************************/ + +int32 GPS_Input_Get_Track(GPS_PTrack **trk, FILE *inf) +{ + char s[GPS_ARB_LEN]; + int32 n; + int32 i; + long pos; + int32 a; + int32 d; + + gps_errno = INPUT_ERROR; + + if(!GPS_Input_Read_Line(s,inf)) + return gps_errno; + if(strncmp(s,"Track log",9)) + return gps_errno; + + if(sscanf(s,"Track log %d%d",(int *)&a,(int *)&d)!=2) + return INPUT_ERROR; + + + if(!GPS_Input_Read_Line(s,inf)) + return gps_errno; + if(strncmp(s,"Start",5)) + return gps_errno; + + pos = ftell(inf); + n = 0; + while(strncmp(s,"End",3)) + { + if(!GPS_Input_Read_Line(s,inf)) + return gps_errno; + if(strstr(s,"Latitude")) + ++n; + if(strstr(s,"Header")) + ++n; + } + fseek(inf,pos,0); + + + if(!(*trk=(GPS_PTrack *)malloc(n*sizeof(GPS_PTrack *)))) + return MEMORY_ERROR; + for(i=0;itnew=1; + if(!GPS_Input_Read_Line(s,inf)) + return gps_errno; + } + else + (*trk)[i]->tnew=0; + + switch(d) + { + case pD300: + GPS_Input_Get_D300(&((*trk)[i]),inf,s); + break; + case pD301: + GPS_Input_Get_D301(&((*trk)[i]),inf,s); + break; + default: + return PROTOCOL_ERROR; + } + } + + return n; +} + + + +/* @funcstatic GPS_Input_Get_Track301 ********************************** +** +** Construct a travk array from a file +** +** @param [w] trk [GPS_PTrack **] pointer to track array +** @param [r] inf [FILE *] stream +** @param [r] type [int32] data type +** @param [r] n [int32] number of tracks +** +** @return [int32] number of entries +************************************************************************/ + +static int32 GPS_Input_Get_Track301(GPS_PTrack **trk, FILE *inf, int32 type, + int32 n) +{ + char s[GPS_ARB_LEN]; + int32 i; + char *p; + int32 x; + + for(i=0;iishdr = 1; + + if(!GPS_Input_Read_Line(s,inf)) + return gps_errno; + GPS_Input_Load_Strnull((*trk)[i]->trk_ident,s); + + if(!GPS_Input_Read_Line(s,inf)) + return gps_errno; + p=strchr(s,':'); + if(sscanf(p+1,"%d",(int *)&x)!=1) + return INPUT_ERROR; + (*trk)[i]->dspl = x; + + if(!GPS_Input_Read_Line(s,inf)) + return gps_errno; + p=strchr(s,':'); + if(sscanf(p+1,"%d",(int *)&x)!=1) + return INPUT_ERROR; + (*trk)[i]->colour = x; + + continue; + } + + (*trk)[i]->ishdr = 0; + + if(!strcmp(s,"New track")) + { + (*trk)[i]->tnew=1; + if(!GPS_Input_Read_Line(s,inf)) + return gps_errno; + } + else + (*trk)[i]->tnew=0; + + switch(type) + { + case pD300: + GPS_Input_Get_D300(&((*trk)[i]),inf,s); + break; + case pD301: + GPS_Input_Get_D301(&((*trk)[i]),inf,s); + break; + default: + return PROTOCOL_ERROR; + } + } + + return n; +} + + + +/* @funcstatic GPS_Input_Get_D300 ********************************** +** +** Construct a track from a file +** +** @param [w] trk [GPS_PTrack *] pointer to track +** @param [r] inf [FILE *] stream +** @param [w] s [char *] input line +** +** @return [int32] number of entries +************************************************************************/ + +static int32 GPS_Input_Get_D300(GPS_PTrack *trk, FILE *inf, char *s) +{ + char *p; + double f; + + p=strchr(s,':'); + if(sscanf(p+1,"%lf",&f)!=1) + return gps_errno; + (*trk)->lat = f; + + if(!GPS_Input_Read_Line(s,inf)) + return gps_errno; + p=strchr(s,':'); + if(sscanf(p+1,"%lf",&f)!=1) + return gps_errno; + (*trk)->lon = f; + + if(!GPS_Input_Read_Line(s,inf)) + return gps_errno; + (*trk)->Time = 0; + + return 1; +} + + + +/* @funcstatic GPS_Input_Get_D301 ********************************** +** +** Construct a track from a file +** +** @param [w] trk [GPS_PTrack *] pointer to track +** @param [r] inf [FILE *] stream +** @param [w] s [char *] input line +** +** @return [int32] number of entries +************************************************************************/ + +static int32 GPS_Input_Get_D301(GPS_PTrack *trk, FILE *inf, char *s) +{ + char *p; + double f; + + p=strchr(s,':'); + if(sscanf(p+1,"%lf",&f)!=1) + return gps_errno; + (*trk)->lat = f; + + if(!GPS_Input_Read_Line(s,inf)) + return gps_errno; + p=strchr(s,':'); + if(sscanf(p+1,"%lf",&f)!=1) + return gps_errno; + (*trk)->lon = f; + + if(!GPS_Input_Read_Line(s,inf)) + return gps_errno; + (*trk)->Time = 0; + + if(!GPS_Input_Read_Line(s,inf)) + return gps_errno; + p=strchr(s,':'); + if(sscanf(p+1,"%lf",&f)!=1) + return gps_errno; + (*trk)->alt = f; + + if(!GPS_Input_Read_Line(s,inf)) + return gps_errno; + p=strchr(s,':'); + if(sscanf(p+1,"%lf",&f)!=1) + return gps_errno; + (*trk)->dpth = f; + + return 1; +} + + + +/* @func GPS_Input_Get_Route ******************************************* +** +** Construct a route array from a file +** +** @param [w] way [GPS_PWay **] pointer to waypoint array +** @param [r] inf [FILE *] stream +** +** @return [int32] number of entries +************************************************************************/ + +int32 GPS_Input_Get_Route(GPS_PWay **way, FILE *inf) +{ + char s[GPS_ARB_LEN]; + int32 n; + int32 type; + int32 rtype=0; + int32 atype=0; + int32 i; + long pos; + int32 ret; + char *p; + int32 d; + + gps_errno = INPUT_ERROR; + + + if(!GPS_Input_Read_Line(s,inf)) + return gps_errno; + if(sscanf(s,"Route log %d",(int *)&atype)!=1) + return gps_errno; + if(!GPS_Input_Read_Line(s,inf)) + return gps_errno; + + switch(atype) + { + case pA200: + break; + case pA201: + return GPS_Input_Get_Route201(way,inf); + default: + GPS_Error("GPS_Input_Get_Route: Unknown protocol"); + return PROTOCOL_ERROR; + } + + + if(!GPS_Input_Read_Line(s,inf)) + return gps_errno; + if(sscanf(s,"Route Type: %d",(int *)&rtype)!=1) + return gps_errno; + + if(!GPS_Input_Read_Line(s,inf)) + return gps_errno; + if(sscanf(s,"Waypoints Type: %d",(int *)&type)!=1) + return gps_errno; + + if(!GPS_Input_Read_Line(s,inf)) + return gps_errno; + if(strncmp(s,"Start",5)) + return gps_errno; + + pos = ftell(inf); + n = 1; + while(strncmp(s,"End",3)) + { + if(!GPS_Input_Read_Line(s,inf)) + return gps_errno; + if(strstr(s,"Latitude") || strstr(s,"Route")) + ++n; + } + fseek(inf,0L,0); + + + if(!(*way=(GPS_PWay *)malloc(n*sizeof(GPS_PWay *)))) + return MEMORY_ERROR; + for(i=0;iprot = type; + } + + if(!GPS_Input_Read_Line(s,inf)) + return gps_errno; + if(!GPS_Input_Read_Line(s,inf)) + return gps_errno; + if(!GPS_Input_Read_Line(s,inf)) + return gps_errno; + + for(i=0;irte_prot = rtype; + (*way)[i]->islink = 0; + if(strstr(s,"Route")) + { + (*way)[i]->isrte = 1; + switch(rtype) + { + case 200: + p = strchr(s,':'); + p = strchr(p+1,':'); + if(sscanf(p+1,"%d",(int *)&d)!=1) + return gps_error; + (*way)[i]->rte_num=d; + break; + case 201: + p = strchr(s,':'); + p = strchr(p+1,':'); + if(sscanf(p+1,"%d",(int *)&d)!=1) + return gps_error; + (*way)[i]->rte_num=d; + ++p; + GPS_Input_Load_String((*way)[i]->rte_cmnt,20,p+2); + break; + case 202: + p = strchr(s,':'); + p = strchr(p+1,':'); + GPS_Input_Load_Strnull((*way)[i]->rte_ident,p+1); + break; + } + if(!GPS_Input_Read_Line(s,inf)) + return gps_errno; + if(!GPS_Input_Read_Line(s,inf)) + return gps_errno; + continue; + } + else + (*way)[i]->isrte=0; + + if(strstr(s,"End")) + { + if(!GPS_Input_Read_Line(s,inf)) + return gps_errno; + + continue; + } + + + switch(type) + { + case 100: + ret = GPS_Input_Get_D100(&((*way)[i]),inf); + if(ret<0) return gps_errno; + break; + case 101: + ret = GPS_Input_Get_D101(&((*way)[i]),inf); + if(ret<0) return gps_errno; + break; + case 102: + ret = GPS_Input_Get_D102(&((*way)[i]),inf); + if(ret<0) return gps_errno; + break; + case 103: + ret = GPS_Input_Get_D103(&((*way)[i]),inf); + if(ret<0) return gps_errno; + break; + case 104: + ret = GPS_Input_Get_D104(&((*way)[i]),inf); + if(ret<0) return gps_errno; + break; + case 105: + ret = GPS_Input_Get_D105(&((*way)[i]),inf); + if(ret<0) return gps_errno; + break; + case 106: + ret = GPS_Input_Get_D106(&((*way)[i]),inf); + if(ret<0) return gps_errno; + break; + case 107: + ret = GPS_Input_Get_D107(&((*way)[i]),inf); + if(ret<0) return gps_errno; + break; + case 108: + ret = GPS_Input_Get_D108(&((*way)[i]),inf); + if(ret<0) return gps_errno; + break; + case 150: + ret = GPS_Input_Get_D150(&((*way)[i]),inf); + if(ret<0) return gps_errno; + break; + case 151: + ret = GPS_Input_Get_D151(&((*way)[i]),inf); + if(ret<0) return gps_errno; + break; + case 152: + ret = GPS_Input_Get_D152(&((*way)[i]),inf); + if(ret<0) return gps_errno; + break; + case 154: + ret = GPS_Input_Get_D154(&((*way)[i]),inf); + if(ret<0) return gps_errno; + break; + case 155: + ret = GPS_Input_Get_D155(&((*way)[i]),inf); + if(ret<0) return gps_errno; + break; + default: + GPS_Error("Input_Get_Waypoints: Unknown protocol"); + return PROTOCOL_ERROR; + } + + } + + return n; +} + + + +/* @funcstatic GPS_Input_Get_Route201 *********************************** +** +** Construct a route array from a file +** +** @param [w] way [GPS_PWay **] pointer to waypoint array +** @param [r] inf [FILE *] stream +** +** @return [int32] number of entries +************************************************************************/ + +static int32 GPS_Input_Get_Route201(GPS_PWay **way, FILE *inf) +{ + char s[GPS_ARB_LEN]; + int32 n; + int32 type; + int32 rtype; + int32 i; + long pos; + int32 ret; + char *p; + int32 d; + + gps_errno = INPUT_ERROR; + + if(!GPS_Input_Read_Line(s,inf)) + return gps_errno; + if(sscanf(s,"Route Type: %d",(int *)&rtype)!=1) + return gps_errno; + + if(!GPS_Input_Read_Line(s,inf)) + return gps_errno; + if(sscanf(s,"Waypoints Type: %d",(int *)&type)!=1) + return gps_errno; + + + pos = ftell(inf); + n = 1; + while(strncmp(s,"End",3)) + { + if(!GPS_Input_Read_Line(s,inf)) + return gps_errno; + if(strstr(s,"Latitude") || strstr(s,"Route") || strstr(s,"Link Class")) + ++n; + } + fseek(inf,0L,0); + + if(!(*way=(GPS_PWay *)malloc(n*sizeof(GPS_PWay *)))) + return MEMORY_ERROR; + for(i=0;iprot = type; + } + + if(!GPS_Input_Read_Line(s,inf)) + return gps_errno; + if(!GPS_Input_Read_Line(s,inf)) + return gps_errno; + if(!GPS_Input_Read_Line(s,inf)) + return gps_errno; + + for(i=0;irte_prot = rtype; + (*way)[i]->islink = 0; + if(strstr(s,"Route")) + { + (*way)[i]->isrte = 1; + switch(rtype) + { + case 200: + p = strchr(s,':'); + p = strchr(p+1,':'); + if(sscanf(p+1,"%d",(int *)&d)!=1) + return gps_error; + (*way)[i]->rte_num=d; + break; + case 201: + p = strchr(s,':'); + p = strchr(p+1,':'); + if(sscanf(p+1,"%d",(int *)&d)!=1) + return gps_error; + (*way)[i]->rte_num=d; + p = strchr(p+1,':'); + GPS_Input_Load_String((*way)[i]->rte_cmnt,20,p+1); + break; + case 202: + p = strchr(s,':'); + p = strchr(p+1,':'); + GPS_Input_Load_Strnull((*way)[i]->rte_ident,p+1); + break; + } + if(!GPS_Input_Read_Line(s,inf)) + return gps_errno; + continue; + } + else + (*way)[i]->isrte=0; + + + + if(strstr(s,"Link Class")) + { + (*way)[i]->islink = 1; + + p = strchr(s,':'); + if(sscanf(p+1,"%d",(int *)&d)!=1) + return gps_error; + (*way)[i]->rte_link_class=d; + + if(!((*way)[i]->rte_link_class==3 || (*way)[i]->rte_link_class + ==0xff)) + { + if(!GPS_Input_Read_Line(s,inf)) + return gps_errno; + GPS_Input_Load_String((*way)[i]->rte_link_subclass,18,s); + } + else + { + GPS_Util_Put_Short((UC *)(*way)[i]->rte_link_subclass,0); + GPS_Util_Put_Int((UC *)(*way)[i]->rte_link_subclass+2,0); + GPS_Util_Put_Uint((UC *)(*way)[i]->rte_link_subclass+6, + 0xffffffff); + GPS_Util_Put_Uint((UC *)(*way)[i]->rte_link_subclass+10, + 0xffffffff); + GPS_Util_Put_Uint((UC *)(*way)[i]->rte_link_subclass+14, + 0xffffffff); + } + + if(!GPS_Input_Read_Line(s,inf)) + return gps_errno; + GPS_Input_Load_Strnull((*way)[i]->rte_link_ident,s); + + continue; + } + else + (*way)[i]->islink=0; + + + if(strstr(s,"End")) + { + GPS_Error("Get_Route201: Unexpected End"); + return INPUT_ERROR; + } + + + switch(type) + { + case 100: + ret = GPS_Input_Get_D100(&((*way)[i]),inf); + if(ret<0) return gps_errno; + break; + case 101: + ret = GPS_Input_Get_D101(&((*way)[i]),inf); + if(ret<0) return gps_errno; + break; + case 102: + ret = GPS_Input_Get_D102(&((*way)[i]),inf); + if(ret<0) return gps_errno; + break; + case 103: + ret = GPS_Input_Get_D103(&((*way)[i]),inf); + if(ret<0) return gps_errno; + break; + case 104: + ret = GPS_Input_Get_D104(&((*way)[i]),inf); + if(ret<0) return gps_errno; + break; + case 105: + ret = GPS_Input_Get_D105(&((*way)[i]),inf); + if(ret<0) return gps_errno; + break; + case 106: + ret = GPS_Input_Get_D106(&((*way)[i]),inf); + if(ret<0) return gps_errno; + break; + case 107: + ret = GPS_Input_Get_D107(&((*way)[i]),inf); + if(ret<0) return gps_errno; + break; + case 108: + ret = GPS_Input_Get_D108(&((*way)[i]),inf); + if(ret<0) return gps_errno; + break; + case 150: + ret = GPS_Input_Get_D150(&((*way)[i]),inf); + if(ret<0) return gps_errno; + break; + case 151: + ret = GPS_Input_Get_D151(&((*way)[i]),inf); + if(ret<0) return gps_errno; + break; + case 152: + ret = GPS_Input_Get_D152(&((*way)[i]),inf); + if(ret<0) return gps_errno; + break; + case 154: + ret = GPS_Input_Get_D154(&((*way)[i]),inf); + if(ret<0) return gps_errno; + break; + case 155: + ret = GPS_Input_Get_D155(&((*way)[i]),inf); + if(ret<0) return gps_errno; + break; + default: + GPS_Error("Input_Get_Waypoints: Unknown protocol"); + return PROTOCOL_ERROR; + } + if(!GPS_Input_Read_Line(s,inf)) + return gps_errno; + } + + return n; +} diff --git a/gpsbabel/jeeps/gpsinput.h b/gpsbabel/jeeps/gpsinput.h new file mode 100644 index 000000000..5b0e41615 --- /dev/null +++ b/gpsbabel/jeeps/gpsinput.h @@ -0,0 +1,23 @@ +#ifdef __cplusplus +extern "C" +{ +#endif + +#ifndef gpsinput_h +#define gpsinput_h + + +#include "gps.h" + +int32 GPS_Input_Get_Almanac(GPS_PAlmanac **alm, FILE *inf); +int32 GPS_Input_Get_Waypoint(GPS_PWay **way, FILE *inf); +int32 GPS_Input_Get_Proximity(GPS_PWay **way, FILE *inf); +int32 GPS_Input_Get_Track(GPS_PTrack **trk, FILE *inf); +int32 GPS_Input_Get_Route(GPS_PWay **way, FILE *inf); + + +#endif + +#ifdef __cplusplus +} +#endif diff --git a/gpsbabel/jeeps/gpsmath.c b/gpsbabel/jeeps/gpsmath.c new file mode 100644 index 000000000..35e9da385 --- /dev/null +++ b/gpsbabel/jeeps/gpsmath.c @@ -0,0 +1,1798 @@ +/******************************************************************** +** @source JEEPS arithmetic/conversion functions +** +** @author Copyright (C) 1999 Alan Bleasby +** @version 1.0 +** @modified Dec 28 1999 Alan Bleasby. First version +** @@ +** +** This library is free software; you can redistribute it and/or +** modify it under the terms of the GNU Library General Public +** License as published by the Free Software Foundation; either +** version 2 of the License, or (at your option) any later version. +** +** This library is distributed in the hope that it will be useful, +** but WITHOUT ANY WARRANTY; without even the implied warranty of +** MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU +** Library General Public License for more details. +** +** You should have received a copy of the GNU Library General Public +** License along with this library; if not, write to the +** Free Software Foundation, Inc., 59 Temple Place - Suite 330, +** Boston, MA 02111-1307, USA. +********************************************************************/ +#include "gps.h" +#include +#include +#include "gpsdatum.h" + + + +static int32 GPS_Math_LatLon_To_UTM_Param(double lat, double lon, int32 *zone, + char *zc, double *Mc, double *E0, + double *N0, double *F0); +static int32 GPS_Math_UTM_Param_To_Mc(int32 zone, char zc, double *Mc, + double *E0, double *N0, double *F0); + + + +/* @func GPS_Math_Deg_To_Rad ******************************************* +** +** Convert degrees to radians +** +** @param [r] v [double] degrees +** +** @return [double] radians +************************************************************************/ + +double GPS_Math_Deg_To_Rad(double v) +{ + return v*(double)((double)GPS_PI/(double)180.); +} + + + +/* @func GPS_Math_Rad_To_Deg ******************************************* +** +** Convert radians to degrees +** +** @param [r] v [double] radians +** +** @return [double] degrees +************************************************************************/ + +double GPS_Math_Rad_To_Deg(double v) +{ + return v*(double)((double)180./(double)GPS_PI); +} + + + +/* @func GPS_Math_Deg_To_DegMin ***************************************** +** +** Convert degrees to degrees and minutes +** +** @param [r] v [double] degrees +** @param [w] d [int32 *] whole degrees +** @param [w] m [double *] minutes +** +** @return [void] +************************************************************************/ + +void GPS_Math_Deg_To_DegMin(double v, int32 *d, double *m) +{ + int32 sign; + + if(v<(double)0.) + { + v *= (double)-1.; + sign = 1; + } + else + sign = 0; + + *d = (int32)v; + *m = (v-(double)*d) * (double)60.0; + if(*m>(double)59.999) + { + ++*d; + *m = (double)0.0; + } + + if(sign) + *d = -*d; + + return; +} + + + +/* @func GPS_Math_DegMin_To_Deg ***************************************** +** +** Convert degrees and minutes to degrees +** +** @param [r] d [int32] whole degrees +** @param [r] m [double] minutes +** @param [w] deg [double *] degrees +** +** @return [void] +************************************************************************/ + +void GPS_Math_DegMin_To_Deg(int32 d, double m, double *deg) +{ + + *deg = ((double)abs(d)) + m / (double)60.0; + if(d<0) + *deg = -*deg; + + return; +} + + + +/* @func GPS_Math_Deg_To_DegMinSec ************************************* +** +** Convert degrees to degrees, minutes and seconds +** +** @param [r] v [double] degrees +** @param [w] d [int32 *] whole degrees +** @param [w] m [int32 *] whole minutes +** @param [w] s [double *] seconds +** +** @return [void] +************************************************************************/ + +void GPS_Math_Deg_To_DegMinSec(double v, int32 *d, int32 *m, double *s) +{ + int32 sign; + double t; + + if(v<(double)0.) + { + v *= (double)-1.; + sign = 1; + } + else + sign = 0; + + *d = (int32)v; + t = (v -(double)*d) * (double)60.0; + *s = (t-(double)*m) * (double)60.0; + + if(*s>(double)59.999) + { + ++t; + *s = (double)0.0; + } + + + if(t>(double)59.999) + { + ++*d; + t = 0; + } + + *m = (int32)t; + + if(sign) + *d = -*d; + + return; +} + + + +/* @func GPS_Math_DegMinSec_To_Deg ***************************************** +** +** Convert degrees, minutes and seconds to degrees +** +** @param [r] d [int32] whole degrees +** @param [r] m [int32] whole minutes +** @param [r] s [double] seconds +** @param [w] deg [double *] degrees +** +** @return [void] +************************************************************************/ + +void GPS_Math_DegMinSec_To_Deg(int32 d, int32 m, double s, double *deg) +{ + + *deg = ((double)abs(d)) + ((double)m + s / (double)60.0) / (double)60.0; + if(d<0) + *deg = -*deg; + + return; +} + + + +/* @func GPS_Math_Metres_To_Feet ******************************************* +** +** Convert metres to feet +** +** @param [r] v [double] metres +** +** @return [double] feet +************************************************************************/ + +double GPS_Math_Metres_To_Feet(double v) +{ + return v*(double)2.7432; +} + + + +/* @func GPS_Math_Feet_To_Metres ******************************************* +** +** Convert feet to metres +** +** @param [r] v [double] feet +** +** @return [double] metres +************************************************************************/ + +double GPS_Math_Feet_To_Metres(double v) +{ + return v/(double)2.7432; +} + + + +/* @func GPS_Math_Deg_To_Semi ******************************************* +** +** Convert degrees to semi-circles +** +** @param [r] v [double] degrees +** +** @return [int32] semicircles +************************************************************************/ + +int32 GPS_Math_Deg_To_Semi(double v) +{ + return (int32) (((double)2.147483e9/(double)180)*(double)v); +} + + + +/* @func GPS_Math_Semi_To_Deg ******************************************* +** +** Convert semi-circles to degrees +** +** @param [r] v [int32] semi-circles +** +** @return [double] degrees +************************************************************************/ + +double GPS_Math_Semi_To_Deg(int32 v) +{ + return (double) (((double)v/(double)2.147483e9) * (double)180); +} + + + +/* @func GPS_Math_Utime_To_Gtime ******************************************* +** +** Convert Unix time (1970) to GPS time (1990) +** +** @param [r] v [time_t] Unix time +** +** @return [time_t] GPS time +************************************************************************/ + +time_t GPS_Math_Utime_To_Gtime(time_t v) +{ + return v-631065600; +} + + + +/* @func GPS_Math_Gtime_To_Utime ******************************************* +** +** Convert GPS time (1990) to Unix time (1970) +** +** @param [r] v [time_t] GPS time +** +** @return [time_t] Unix time +************************************************************************/ + +time_t GPS_Math_Gtime_To_Utime(time_t v) +{ + return v+631065600; +} + + + + +/* @func GPS_Math_LatLonH_To_XYZ ********************************** +** +** Convert latitude and longitude and ellipsoidal height to +** X, Y & Z coordinates +** +** @param [r] phi [double] latitude (deg) +** @param [r] lambda [double] longitude (deg) +** @param [r] H [double] ellipsoidal height (metres) +** @param [w] x [double *] X +** @param [w] y [double *] Y +** @param [w] z [double *] Z +** @param [r] a [double] semi-major axis (metres) +** @param [r] b [double] semi-minor axis (metres) +** +** @return [void] +************************************************************************/ +void GPS_Math_LatLonH_To_XYZ(double phi, double lambda, double H, + double *x, double *y, double *z, + double a, double b) +{ + double esq; + double nu; + + phi = GPS_Math_Deg_To_Rad(phi); + lambda = GPS_Math_Deg_To_Rad(lambda); + + + esq = ((a*a)-(b*b)) / (a*a); + + nu = a / pow(((double)1.0-esq*sin(phi)*sin(phi)),(double)0.5); + *x = (nu+H) * cos(phi) * cos(lambda); + *y = (nu+H) * cos(phi) * sin(lambda); + *z = (((double)1.0-esq)*nu+H) * sin(phi); + + return; +} + + + + +/* @func GPS_Math_XYX_To_LatLonH *************************************** +** +** Convert XYZ coordinates to latitude and longitude and ellipsoidal height +** +** @param [w] phi [double] latitude (deg) +** @param [w] lambda [double] longitude (deg) +** @param [w] H [double] ellipsoidal height (metres) +** @param [r] x [double *] X +** @param [r] y [double *] Y +** @param [r] z [double *] Z +** @param [r] a [double] semi-major axis (metres) +** @param [r] b [double] semi-minor axis (metres) +** +** @return [void] +************************************************************************/ +void GPS_Math_XYZ_To_LatLonH(double *phi, double *lambda, double *H, + double x, double y, double z, + double a, double b) +{ + double esq; + double nu=0.0; + double phix; + double nphi; + double rho; + int32 t1=0; + int32 t2=0; + + if(x<(double)0 && y>=(double)0) t1=1; + if(x<(double)0 && y<(double)0) t2=1; + + rho = pow(((x*x)+(y*y)),(double)0.5); + esq = ((a*a)-(b*b)) / (a*a); + phix = atan(z/(((double)1.0 - esq) * rho)); + nphi = (double)1e20; + + while(fabs(phix-nphi)>(double)0.00000000001) + { + nphi = phix; + nu = a / pow(((double)1.0-esq*sin(nphi)*sin(nphi)),(double)0.5); + phix = atan((z+esq*nu*sin(nphi))/rho); + } + + *phi = GPS_Math_Rad_To_Deg(phix); + *H = (rho / cos(phix)) - nu; + *lambda = GPS_Math_Rad_To_Deg(atan(y/x)); + + if(t1) *lambda += (double)180.0; + if(t2) *lambda -= (double)180.0; + + return; +} + + + +/* @func GPS_Math_Airy1830LatLonH_To_XYZ ********************************** +** +** Convert Airy 1830 latitude and longitude and ellipsoidal height to +** X, Y & Z coordinates +** +** @param [r] phi [double] latitude (deg) +** @param [r] lambda [double] longitude (deg) +** @param [r] H [double] ellipsoidal height (metres) +** @param [w] x [double *] X +** @param [w] y [double *] Y +** @param [w] z [double *] Z +** +** @return [void] +************************************************************************/ +void GPS_Math_Airy1830LatLonH_To_XYZ(double phi, double lambda, double H, + double *x, double *y, double *z) +{ + double a = 6377563.396; + double b = 6356256.910; + + GPS_Math_LatLonH_To_XYZ(phi,lambda,H,x,y,z,a,b); + + return; +} + + + +/* @func GPS_Math_WGS84LatLonH_To_XYZ ********************************** +** +** Convert WGS84 latitude and longitude and ellipsoidal height to +** X, Y & Z coordinates +** +** @param [r] phi [double] latitude (deg) +** @param [r] lambda [double] longitude (deg) +** @param [r] H [double] ellipsoidal height (metres) +** @param [w] x [double *] X +** @param [w] y [double *] Y +** @param [w] z [double *] Z +** +** @return [void] +************************************************************************/ +void GPS_Math_WGS84LatLonH_To_XYZ(double phi, double lambda, double H, + double *x, double *y, double *z) +{ + double a = 6378137.000; + double b = 6356752.3141; + + GPS_Math_LatLonH_To_XYZ(phi,lambda,H,x,y,z,a,b); + + return; +} + + + + +/* @func GPS_Math_XYZ_To_Airy1830LatLonH ********************************** +** +** Convert XYZ to Airy 1830 latitude and longitude and ellipsoidal height +** +** @param [r] phi [double] latitude (deg) +** @param [r] lambda [double] longitude (deg) +** @param [r] H [double] ellipsoidal height (metres) +** @param [w] x [double *] X +** @param [w] y [double *] Y +** @param [w] z [double *] Z +** +** @return [void] +************************************************************************/ +void GPS_Math_XYZ_To_Airy1830LatLonH(double *phi, double *lambda, double *H, + double x, double y, double z) +{ + double a = 6377563.396; + double b = 6356256.910; + + GPS_Math_XYZ_To_LatLonH(phi,lambda,H,x,y,z,a,b); + + return; +} + + + +/* @func GPS_Math_XYZ_To_WGS84LatLonH ********************************** +** +** Convert XYZ to WGS84 latitude and longitude and ellipsoidal height +** +** @param [r] phi [double] latitude (deg) +** @param [r] lambda [double] longitude (deg) +** @param [r] H [double] ellipsoidal height (metres) +** @param [w] x [double *] X +** @param [w] y [double *] Y +** @param [w] z [double *] Z +** +** @return [void] +************************************************************************/ +void GPS_Math_XYZ_To_WGS84LatLonH(double *phi, double *lambda, double *H, + double x, double y, double z) +{ + double a = 6378137.000; + double b = 66356752.3141; + + GPS_Math_XYZ_To_LatLonH(phi,lambda,H,x,y,z,a,b); + + return; +} + + + + + + + +/* @func GPS_Math_LatLon_To_EN ********************************** +** +** Convert latitude and longitude to eastings and northings +** Standard Gauss-Kruger Transverse Mercator +** +** @param [w] E [double *] easting (metres) +** @param [w] N [double *] northing (metres) +** @param [r] phi [double] latitude (deg) +** @param [r] lambda [double] longitude (deg) +** @param [r] N0 [double] true northing origin (metres) +** @param [r] E0 [double] true easting origin (metres) +** @param [r] phi0 [double] true latitude origin (deg) +** @param [r] lambda0 [double] true longitude origin (deg) +** @param [r] F0 [double] scale factor on central meridian +** @param [r] a [double] semi-major axis (metres) +** @param [r] b [double] semi-minor axis (metres) +** +** @return [void] +************************************************************************/ +void GPS_Math_LatLon_To_EN(double *E, double *N, double phi, + double lambda, double N0, double E0, + double phi0, double lambda0, + double F0, double a, double b) +{ + double esq; + double n; + double etasq; + double nu; + double rho; + double M; + double I; + double II; + double III; + double IIIA; + double IV; + double V; + double VI; + + double tmp; + double tmp2; + double fdf; + double fde; + + phi0 = GPS_Math_Deg_To_Rad(phi0); + lambda0 = GPS_Math_Deg_To_Rad(lambda0); + phi = GPS_Math_Deg_To_Rad(phi); + lambda = GPS_Math_Deg_To_Rad(lambda); + + esq = ((a*a)-(b*b)) / (a*a); + n = (a-b) / (a+b); + + tmp = (double)1.0 - (esq * sin(phi) * sin(phi)); + nu = a * F0 * pow(tmp,(double)-0.5); + rho = a * F0 * ((double)1.0 - esq) * pow(tmp,(double)-1.5); + etasq = (nu / rho) - (double)1.0; + + fdf = (double)5.0 / (double)4.0; + tmp = (double)1.0 + n + (fdf * n * n) + (fdf * n * n * n); + tmp *= (phi - phi0); + tmp2 = (double)3.0*n + (double)3.0*n*n + ((double)21./(double)8.)*n*n*n; + tmp2 *= (sin(phi-phi0) * cos(phi+phi0)); + tmp -= tmp2; + + fde = ((double)15.0 / (double)8.0); + tmp2 = ((fde*n*n) + (fde*n*n*n)) * sin((double)2.0 * (phi-phi0)); + tmp2 *= cos((double)2.0 * (phi+phi0)); + tmp += tmp2; + + tmp2 = ((double)35.0/(double)24.0) * n * n * n; + tmp2 *= sin((double)3.0 * (phi-phi0)); + tmp2 *= cos((double)3.0 * (phi+phi0)); + tmp -= tmp2; + + M = b * F0 * tmp; + I = M + N0; + II = (nu / (double)2.0) * sin(phi) * cos(phi); + III = (nu / (double)24.0) * sin(phi) * cos(phi) * cos(phi) * cos(phi); + III *= ((double)5.0 - (tan(phi) * tan(phi)) + ((double)9.0 * etasq)); + IIIA = (nu / (double)720.0) * sin(phi) * pow(cos(phi),(double)5.0); + IIIA *= ((double)61.0 - ((double)58.0*tan(phi)*tan(phi)) + + pow(tan(phi),(double)4.0)); + IV = nu * cos(phi); + + tmp = pow(cos(phi),(double)3.0); + tmp *= ((nu/rho) - tan(phi) * tan(phi)); + V = (nu/(double)6.0) * tmp; + + tmp = (double)5.0 - ((double)18.0 * tan(phi) * tan(phi)); + tmp += tan(phi)*tan(phi)*tan(phi)*tan(phi) + ((double)14.0 * etasq); + tmp -= ((double)58.0 * tan(phi) * tan(phi) * etasq); + tmp2 = cos(phi)*cos(phi)*cos(phi)*cos(phi)*cos(phi) * tmp; + VI = (nu / (double)120.0) * tmp2; + + *N = I + II*(lambda-lambda0)*(lambda-lambda0) + + III*pow((lambda-lambda0),(double)4.0) + + IIIA*pow((lambda-lambda0),(double)6.0); + + *E = E0 + IV*(lambda-lambda0) + V*pow((lambda-lambda0),(double)3.0) + + VI * pow((lambda-lambda0),(double)5.0); + + return; +} + + + +/* @func GPS_Math_Airy1830MLatLonToINGEN ************************************ +** +** Convert Modified Airy 1830 datum latitude and longitude to Irish +** National Grid Eastings and Northings +** +** @param [r] phi [double] modified Airy latitude (deg) +** @param [r] lambda [double] modified Airy longitude (deg) +** @param [w] E [double *] NG easting (metres) +** @param [w] N [double *] NG northing (metres) +** +** @return [void] +************************************************************************/ +void GPS_Math_Airy1830M_LatLonToINGEN(double phi, double lambda, double *E, + double *N) +{ + double N0 = 250000; + double E0 = 200000; + double F0 = 1.000035; + double phi0 = 53.5; + double lambda0 = -8.; + double a = 6377340.189; + double b = 6356034.447; + + GPS_Math_LatLon_To_EN(E,N,phi,lambda,N0,E0,phi0,lambda0,F0,a,b); + + return; +} + + + + +/* @func GPS_Math_Airy1830LatLonToNGEN ************************************** +** +** Convert Airy 1830 datum latitude and longitude to UK Ordnance Survey +** National Grid Eastings and Northings +** +** @param [r] phi [double] WGS84 latitude (deg) +** @param [r] lambda [double] WGS84 longitude (deg) +** @param [w] E [double *] NG easting (metres) +** @param [w] N [double *] NG northing (metres) +** +** @return [void] +************************************************************************/ +void GPS_Math_Airy1830LatLonToNGEN(double phi, double lambda, double *E, + double *N) +{ + double N0 = -100000; + double E0 = 400000; + double F0 = 0.9996012717; + double phi0 = 49.; + double lambda0 = -2.; + double a = 6377563.396; + double b = 6356256.910; + + GPS_Math_LatLon_To_EN(E,N,phi,lambda,N0,E0,phi0,lambda0,F0,a,b); + + return; +} + + + + +/* @func GPS_Math_EN_To_LatLon ************************************** +** +** Convert Eastings and Northings to latitude and longitude +** +** @param [w] E [double] NG easting (metres) +** @param [w] N [double] NG northing (metres) +** @param [r] phi [double *] Airy latitude (deg) +** @param [r] lambda [double *] Airy longitude (deg) +** @param [r] N0 [double] true northing origin (metres) +** @param [r] E0 [double] true easting origin (metres) +** @param [r] phi0 [double] true latitude origin (deg) +** @param [r] lambda0 [double] true longitude origin (deg) +** @param [r] F0 [double] scale factor on central meridian +** @param [r] a [double] semi-major axis (metres) +** @param [r] b [double] semi-minor axis (metres) +** +** @return [void] +************************************************************************/ +void GPS_Math_EN_To_LatLon(double E, double N, double *phi, + double *lambda, double N0, double E0, + double phi0, double lambda0, + double F0, double a, double b) +{ + double esq; + double n; + double etasq; + double nu; + double rho; + double M; + double VII; + double VIII; + double IX; + double X; + double XI; + double XII; + double XIIA; + double phix; + double nphi=0.0; + + double tmp; + double tmp2; + double fdf; + double fde; + + phi0 = GPS_Math_Deg_To_Rad(phi0); + lambda0 = GPS_Math_Deg_To_Rad(lambda0); + + n = (a-b) / (a+b); + fdf = (double)5.0 / (double)4.0; + fde = ((double)15.0 / (double)8.0); + + esq = ((a*a)-(b*b)) / (a*a); + + + phix = ((N-N0)/(a*F0)) + phi0; + + tmp = (double)1.0 - (esq * sin(phix) * sin(phix)); + nu = a * F0 * pow(tmp,(double)-0.5); + rho = a * F0 * ((double)1.0 - esq) * pow(tmp,(double)-1.5); + etasq = (nu / rho) - (double)1.0; + + M = (double)-1e20; + + while(N-N0-M > (double)0.000001) + { + nphi = phix; + + tmp = (double)1.0 + n + (fdf * n * n) + (fdf * n * n * n); + tmp *= (nphi - phi0); + tmp2 = (double)3.0*n + (double)3.0*n*n + + ((double)21./(double)8.)*n*n*n; + tmp2 *= (sin(nphi-phi0) * cos(nphi+phi0)); + tmp -= tmp2; + + + tmp2 = ((fde*n*n) + (fde*n*n*n)) * sin((double)2.0 * (nphi-phi0)); + tmp2 *= cos((double)2.0 * (nphi+phi0)); + tmp += tmp2; + + tmp2 = ((double)35.0/(double)24.0) * n * n * n; + tmp2 *= sin((double)3.0 * (nphi-phi0)); + tmp2 *= cos((double)3.0 * (nphi+phi0)); + tmp -= tmp2; + + M = b * F0 * tmp; + + if(N-N0-M > (double)0.000001) + phix = ((N-N0-M)/(a*F0)) + nphi; + } + + + VII = tan(nphi) / ((double)2.0 * rho * nu); + + tmp = (double)5.0 + (double)3.0 * tan(nphi) * tan(nphi) + etasq; + tmp -= (double)9.0 * tan(nphi) * tan(nphi) * etasq; + VIII = (tan(nphi)*tmp) / ((double)24.0 * rho * nu*nu*nu); + + tmp = (double)61.0 + (double)90.0 * tan(nphi) * tan(nphi); + tmp += (double)45.0 * pow(tan(nphi),(double)4.0); + IX = tan(nphi) / ((double)720.0 * rho * pow(nu,(double)5.0)) * tmp; + + X = (double)1.0 / (cos(nphi) * nu); + + tmp = (nu / rho) + (double)2.0 * tan(nphi) * tan(nphi); + XI = ((double)1.0 / (cos(nphi) * (double)6.0 * nu*nu*nu)) * tmp; + + tmp = (double)5.0 + (double)28.0 * tan(nphi)*tan(nphi); + tmp += (double)24.0 * pow(tan(nphi),(double)4.0); + XII = ((double)1.0 / ((double)120.0 * pow(nu,(double)5.0) * cos(nphi))) + * tmp; + + tmp = (double)61.0 + (double)662.0 * tan(nphi) * tan(nphi); + tmp += (double)1320.0 * pow(tan(nphi),(double)4.0); + tmp += (double)720.0 * pow(tan(nphi),(double)6.0); + XIIA = ((double)1.0 / (cos(nphi) * (double)5040.0 * pow(nu,(double)7.0))) + * tmp; + + *phi = nphi - VII*pow((E-E0),(double)2.0) + VIII*pow((E-E0),(double)4.0) - + IX*pow((E-E0),(double)6.0); + + *lambda = lambda0 + X*(E-E0) - XI*pow((E-E0),(double)3.0) + + XII*pow((E-E0),(double)5.0) - XIIA*pow((E-E0),(double)7.0); + + *phi = GPS_Math_Rad_To_Deg(*phi); + *lambda = GPS_Math_Rad_To_Deg(*lambda); + + return; +} + + + + +/* @func GPS_Math_NGENToAiry1830LatLon ************************************** +** +** Convert to UK Ordnance Survey National Grid Eastings and Northings to +** Airy 1830 datum latitude and longitude +** +** @param [r] E [double] NG easting (metres) +** @param [r] N [double] NG northing (metres) +** @param [w] phi [double *] Airy latitude (deg) +** @param [w] lambda [double *] Airy longitude (deg) +** +** @return [void] +************************************************************************/ +void GPS_Math_NGENToAiry1830LatLon(double E, double N, double *phi, + double *lambda) +{ + double N0 = -100000; + double E0 = 400000; + double F0 = 0.9996012717; + double phi0 = 49.; + double lambda0 = -2.; + double a = 6377563.396; + double b = 6356256.910; + + GPS_Math_EN_To_LatLon(E,N,phi,lambda,N0,E0,phi0,lambda0,F0,a,b); + + return; +} + + + +/* @func GPS_Math_INGENToAiry1830MLatLon ************************************** +** +** Convert Irish National Grid Eastings and Northings to modified +** Airy 1830 datum latitude and longitude +** +** @param [r] E [double] ING easting (metres) +** @param [r] N [double] ING northing (metres) +** @param [w] phi [double *] modified Airy latitude (deg) +** @param [w] lambda [double *] modified Airy longitude (deg) +** +** @return [void] +************************************************************************/ +void GPS_Math_INGENToAiry1830MLatLon(double E, double N, double *phi, + double *lambda) +{ + double N0 = 250000; + double E0 = 200000; + double F0 = 1.000035; + double phi0 = 53.5; + double lambda0 = -8.; + double a = 6377340.189; + double b = 6356034.447; + + GPS_Math_EN_To_LatLon(E,N,phi,lambda,N0,E0,phi0,lambda0,F0,a,b); + + return; +} + + + +/* @func GPS_Math_EN_To_UKOSNG_Map ************************************* +** +** Convert Airy 1830 eastings and northings to Ordnance Survey map +** two letter code plus modified eastings and northings +** +** @param [r] E [double] NG easting (metres) +** @param [r] N [double] NG northing (metres) +** @param [w] mE [double *] modified easting (metres) +** @param [w] mN [double *] modified northing (metres) +** @param [w] map [char *] map code +** +** @return [int32] success +************************************************************************/ +int32 GPS_Math_EN_To_UKOSNG_Map(double E, double N, double *mE, + double *mN, char *map) +{ + int32 t; + int32 idx; + + if(E>=(double)700000. || E<(double)0.0 || N<(double)0.0 || + N>=(double)1300000.0) + return 0; + + idx = ((int32)N/100000)*7 + (int32)E/100000; + (void) strcpy(map,UKNG[idx]); + + t = ((int32)E / 100000) * 100000; + *mE = E - (double)t; + + t = ((int32)N / 100000) * 100000; + *mN = N - (double)t; + + return 1; +} + + + +/* @func GPS_Math_UKOSNG_Map_To_EN ************************************* +** +** Convert Ordnance Survey map eastings and northings plus +** two letter code to Airy 1830 eastings and northings +** +** @param [w] map [char *] map code +** @param [r] mapE [double] easting (metres) +** @param [r] mapN [double] northing (metres) +** @param [w] E [double *] full Airy easting (metres) +** @param [w] N [double *] full Airy northing (metres) + +** +** @return [int32] success +************************************************************************/ +int32 GPS_Math_UKOSNG_Map_To_EN(char *map, double mapE, double mapN, double *E, + double *N) +{ + int32 t; + int32 idx; + + if(mapE>=(double)100000.0 || mapE<(double)0.0 || mapN<(double)0.0 || + mapN>(double)100000.0) + return 0; + + idx=0; + while(*UKNG[idx]) + { + if(!strcmp(UKNG[idx],map)) break; + ++idx; + } + if(!*UKNG[idx]) + return 0; + + + t = (idx / 7) * 100000; + *N = mapN + (double)t; + + t = (idx % 7) * 100000; + *E = mapE + (double)t; + + return 1; +} + + + +/* @func GPS_Math_Molodensky ******************************************* +** +** Transform one datum to another +** +** @param [r] Sphi [double] source latitude (deg) +** @param [r] Slam [double] source longitude (deg) +** @param [r] SH [double] source height (metres) +** @param [r] Sa [double] source semi-major axis (metres) +** @param [r] Sif [double] source inverse flattening +** @param [w] Dphi [double *] dest latitude (deg) +** @param [w] Dlam [double *] dest longitude (deg) +** @param [w] DH [double *] dest height (metres) +** @param [r] Da [double] dest semi-major axis (metres) +** @param [r] Dif [double] dest inverse flattening +** @param [r] dx [double] dx +** @param [r] dy [double] dy +** @param [r] dz [double] dz +** +** @return [void] +************************************************************************/ +void GPS_Math_Molodensky(double Sphi, double Slam, double SH, double Sa, + double Sif, double *Dphi, double *Dlam, + double *DH, double Da, double Dif, double dx, + double dy, double dz) +{ + double Sf; + double Df; + double esq; + double bda; + double da; + double df; + double N; + double M; + double tmp; + double tmp2; + double dphi; + double dlambda; + double dheight; + double phis; + double phic; + double lams; + double lamc; + + Sf = (double)1.0 / Sif; + Df = (double)1.0 / Dif; + + esq = (double)2.0*Sf - pow(Sf,(double)2.0); + bda = (double)1.0 - Sf; + Sphi = GPS_Math_Deg_To_Rad(Sphi); + Slam = GPS_Math_Deg_To_Rad(Slam); + + da = Da - Sa; + df = Df - Sf; + + phis = sin(Sphi); + phic = cos(Sphi); + lams = sin(Slam); + lamc = cos(Slam); + + N = Sa / sqrt((double)1.0 - esq*pow(phis,(double)2.0)); + + tmp = ((double)1.0-esq) /pow(((double)1.0-esq*pow(phis,(double)2.0)),1.5); + M = Sa * tmp; + + tmp = df * ((M/bda)+N*bda) * phis * phic; + tmp2 = da * N * esq * phis * phic / Sa; + tmp2 += ((-dx*phis*lamc-dy*phis*lams) + dz*phic); + dphi = (tmp2 + tmp) / (M + SH); + + dlambda = (-dx*lams+dy*lamc) / ((N+SH)*phic); + + dheight = dx*phic*lamc + dy*phic*lams + dz*phis - da*(Sa/N) + + df*bda*N*phis*phis; + + *Dphi = Sphi + dphi; + *Dlam = Slam + dlambda; + *DH = SH + dheight; + + *Dphi = GPS_Math_Rad_To_Deg(*Dphi); + *Dlam = GPS_Math_Rad_To_Deg(*Dlam); + + return; +} + + + +/* @func GPS_Math_Known_Datum_To_WGS84_M ********************************** +** +** Transform datum to WGS84 using Molodensky +** +** @param [r] Sphi [double] source latitude (deg) +** @param [r] Slam [double] source longitude (deg) +** @param [r] SH [double] source height (metres) +** @param [w] Dphi [double *] dest latitude (deg) +** @param [w] Dlam [double *] dest longitude (deg) +** @param [w] DH [double *] dest height (metres) +** @param [r] n [int32] datum number from GPS_Datum structure +** +** @return [void] +************************************************************************/ +void GPS_Math_Known_Datum_To_WGS84_M(double Sphi, double Slam, double SH, + double *Dphi, double *Dlam, double *DH, + int32 n) +{ + double Sa; + double Sif; + double Da; + double Dif; + double x; + double y; + double z; + int32 idx; + + Da = (double) 6378137.0; + Dif = (double) 298.257223563; + + idx = GPS_Datum[n].ellipse; + Sa = GPS_Ellipse[idx].a; + Sif = GPS_Ellipse[idx].invf; + x = GPS_Datum[n].dx; + y = GPS_Datum[n].dy; + z = GPS_Datum[n].dz; + + GPS_Math_Molodensky(Sphi,Slam,SH,Sa,Sif,Dphi,Dlam,DH,Da,Dif,x,y,z); + + return; +} + + + +/* @func GPS_Math_WGS84_To_Known_Datum_M ******************************** +** +** Transform WGS84 to other datum using Molodensky +** +** @param [r] Sphi [double] source latitude (deg) +** @param [r] Slam [double] source longitude (deg) +** @param [r] SH [double] source height (metres) +** @param [w] Dphi [double *] dest latitude (deg) +** @param [w] Dlam [double *] dest longitude (deg) +** @param [w] DH [double *] dest height (metres) +** @param [r] n [int32] datum number from GPS_Datum structure +** +** @return [void] +************************************************************************/ +void GPS_Math_WGS84_To_Known_Datum_M(double Sphi, double Slam, double SH, + double *Dphi, double *Dlam, double *DH, + int32 n) +{ + double Sa; + double Sif; + double Da; + double Dif; + double x; + double y; + double z; + int32 idx; + + Sa = (double) 6378137.0; + Sif = (double) 298.257223563; + + idx = GPS_Datum[n].ellipse; + Da = GPS_Ellipse[idx].a; + Dif = GPS_Ellipse[idx].invf; + x = -GPS_Datum[n].dx; + y = -GPS_Datum[n].dy; + z = -GPS_Datum[n].dz; + + GPS_Math_Molodensky(Sphi,Slam,SH,Sa,Sif,Dphi,Dlam,DH,Da,Dif,x,y,z); + + return; +} + + + +/* @func GPS_Math_Known_Datum_To_WGS84_C ********************************** +** +** Transform datum to WGS84 using Cartesian coordinates +** +** @param [r] Sphi [double] source latitude (deg) +** @param [r] Slam [double] source longitude (deg) +** @param [r] SH [double] source height (metres) +** @param [w] Dphi [double *] dest latitude (deg) +** @param [w] Dlam [double *] dest longitude (deg) +** @param [w] DH [double *] dest height (metres) +** @param [r] n [int32] datum number from GPS_Datum structure +** +** @return [void] +************************************************************************/ +void GPS_Math_Known_Datum_To_WGS84_C(double Sphi, double Slam, double SH, + double *Dphi, double *Dlam, double *DH, + int32 n) +{ + double Sa; + double Sif; + double Sb; + double Da; + double Dif; + double Db; + double x; + double y; + double z; + int32 idx; + double sx; + double sy; + double sz; + + Da = (double) 6378137.0; + Dif = (double) 298.257223563; + Db = Da - (Da / Dif); + + idx = GPS_Datum[n].ellipse; + Sa = GPS_Ellipse[idx].a; + Sif = GPS_Ellipse[idx].invf; + Sb = Sa - (Sa / Sif); + + x = GPS_Datum[n].dx; + y = GPS_Datum[n].dy; + z = GPS_Datum[n].dz; + + GPS_Math_LatLonH_To_XYZ(Sphi,Slam,SH,&sx,&sy,&sz,Sa,Sb); + sx += x; + sy += y; + sz += z; + + GPS_Math_XYZ_To_LatLonH(Dphi,Dlam,DH,sx,sy,sz,Da,Db); + + return; +} + + + +/* @func GPS_Math_WGS84_To_Known_Datum_C ******************************** +** +** Transform WGS84 to other datum using Cartesian coordinates +** +** @param [r] Sphi [double] source latitude (deg) +** @param [r] Slam [double] source longitude (deg) +** @param [r] SH [double] source height (metres) +** @param [w] Dphi [double *] dest latitude (deg) +** @param [w] Dlam [double *] dest longitude (deg) +** @param [w] DH [double *] dest height (metres) +** @param [r] n [int32] datum number from GPS_Datum structure +** +** @return [void] +************************************************************************/ +void GPS_Math_WGS84_To_Known_Datum_C(double Sphi, double Slam, double SH, + double *Dphi, double *Dlam, double *DH, + int32 n) +{ + double Sa; + double Sif; + double Da; + double Dif; + double x; + double y; + double z; + int32 idx; + double Sb; + double Db; + double dx; + double dy; + double dz; + + Sa = (double) 6378137.0; + Sif = (double) 298.257223563; + Sb = Sa - (Sa / Sif); + + idx = GPS_Datum[n].ellipse; + Da = GPS_Ellipse[idx].a; + Dif = GPS_Ellipse[idx].invf; + Db = Da - (Da / Dif); + + x = -GPS_Datum[n].dx; + y = -GPS_Datum[n].dy; + z = -GPS_Datum[n].dz; + + GPS_Math_LatLonH_To_XYZ(Sphi,Slam,SH,&dx,&dy,&dz,Sa,Sb); + dx += x; + dy += y; + dz += z; + + GPS_Math_XYZ_To_LatLonH(Dphi,Dlam,DH,dx,dy,dz,Da,Db); + + return; +} + + + +/* @func GPS_Math_Known_Datum_To_Known_Datum_M ************************* +** +** Transform WGS84 to other datum using Molodensky +** +** @param [r] Sphi [double] source latitude (deg) +** @param [r] Slam [double] source longitude (deg) +** @param [r] SH [double] source height (metres) +** @param [w] Dphi [double *] dest latitude (deg) +** @param [w] Dlam [double *] dest longitude (deg) +** @param [w] DH [double *] dest height (metres) +** @param [r] n1 [int32] source datum number from GPS_Datum structure +** @param [r] n2 [int32] dest datum number from GPS_Datum structure +** +** @return [void] +************************************************************************/ +void GPS_Math_Known_Datum_To_Known_Datum_M(double Sphi, double Slam, double SH, + double *Dphi, double *Dlam, + double *DH, int32 n1, int32 n2) +{ + double Sa; + double Sif; + double Da; + double Dif; + double x1; + double y1; + double z1; + double x2; + double y2; + double z2; + double x; + double y; + double z; + + int32 idx1; + int32 idx2; + + + idx1 = GPS_Datum[n1].ellipse; + Sa = GPS_Ellipse[idx1].a; + Sif = GPS_Ellipse[idx1].invf; + x1 = GPS_Datum[n1].dx; + y1 = GPS_Datum[n1].dy; + z1 = GPS_Datum[n1].dz; + + idx2 = GPS_Datum[n2].ellipse; + Da = GPS_Ellipse[idx2].a; + Dif = GPS_Ellipse[idx2].invf; + x2 = GPS_Datum[n2].dx; + y2 = GPS_Datum[n2].dy; + z2 = GPS_Datum[n2].dz; + + x = -(x2-x1); + y = -(y2-y1); + z = -(z2-z1); + + GPS_Math_Molodensky(Sphi,Slam,SH,Sa,Sif,Dphi,Dlam,DH,Da,Dif,x,y,z); + + return; +} + + + +/* @func GPS_Math_Known_Datum_To_Known_Datum_C ************************* +** +** Transform known datum to other datum using Cartesian coordinates +** +** @param [r] Sphi [double] source latitude (deg) +** @param [r] Slam [double] source longitude (deg) +** @param [r] SH [double] source height (metres) +** @param [w] Dphi [double *] dest latitude (deg) +** @param [w] Dlam [double *] dest longitude (deg) +** @param [w] DH [double *] dest height (metres) +** @param [r] n1 [int32] source datum number from GPS_Datum structure +** @param [r] n2 [int32] dest datum number from GPS_Datum structure +** +** @return [void] +************************************************************************/ +void GPS_Math_Known_Datum_To_Known_Datum_C(double Sphi, double Slam, double SH, + double *Dphi, double *Dlam, + double *DH, int32 n1, int32 n2) +{ + double Sa; + double Sif; + double Da; + double Dif; + double x1; + double y1; + double z1; + double x2; + double y2; + double z2; + + int32 idx1; + int32 idx2; + + double Sb; + double Db; + double dx; + double dy; + double dz; + + idx1 = GPS_Datum[n1].ellipse; + Sa = GPS_Ellipse[idx1].a; + Sif = GPS_Ellipse[idx1].invf; + Sb = Sa - (Sa / Sif); + + x1 = GPS_Datum[n1].dx; + y1 = GPS_Datum[n1].dy; + z1 = GPS_Datum[n1].dz; + + idx2 = GPS_Datum[n2].ellipse; + Da = GPS_Ellipse[idx2].a; + Dif = GPS_Ellipse[idx2].invf; + Db = Da - (Da / Dif); + + x2 = GPS_Datum[n2].dx; + y2 = GPS_Datum[n2].dy; + z2 = GPS_Datum[n2].dz; + + GPS_Math_LatLonH_To_XYZ(Sphi,Slam,SH,&dx,&dy,&dz,Sa,Sb); + dx += -(x2-x1); + dy += -(y2-y1); + dz += -(z2-z1); + + GPS_Math_XYZ_To_LatLonH(Dphi,Dlam,DH,dx,dy,dz,Da,Db); + + return; +} + + + +/* @func GPS_Math_WGS84_To_UKOSMap_M *********************************** +** +** Convert WGS84 lat/lon to Ordnance survey map code and easting and +** northing. Uses Molodensky +** +** @param [r] lat [double] WGS84 latitude (deg) +** @param [r] lon [double] WGS84 longitude (deg) +** @param [w] mE [double *] map easting (metres) +** @param [w] mN [double *] map northing (metres) +** @param [w] map [char *] map two letter code +** +** @return [int32] success +************************************************************************/ +int32 GPS_Math_WGS84_To_UKOSMap_M(double lat, double lon, double *mE, + double *mN, char *map) +{ + double alat; + double alon; + double aht; + double aE; + double aN; + + + GPS_Math_WGS84_To_Known_Datum_M(lat,lon,30,&alat,&alon,&aht,86); + + GPS_Math_Airy1830LatLonToNGEN(alat,alon,&aE,&aN); + + if(!GPS_Math_EN_To_UKOSNG_Map(aE,aN,mE,mN,map)) + return 0; + + return 1; +} + + + +/* @func GPS_Math_UKOSMap_To_WGS84_M *********************************** +** +** Transform UK Ordnance survey map position to WGS84 lat/lon +** Uses Molodensky transformation +** +** @param [r] map [char *] map two letter code +** @param [r] mE [double] map easting (metres) +** @param [r] mN [double] map northing (metres) +** @param [w] lat [double *] WGS84 latitude (deg) +** @param [w] lon [double *] WGS84 longitude (deg) +** +** @return [int32] success +************************************************************************/ +int32 GPS_Math_UKOSMap_To_WGS84_M(char *map, double mE, double mN, + double *lat, double *lon) +{ + double E; + double N; + double alat; + double alon; + double ht; + + if(!GPS_Math_UKOSNG_Map_To_EN(map,mE,mN,&E,&N)) + return 0; + + GPS_Math_NGENToAiry1830LatLon(E,N,&alat,&alon); + + GPS_Math_Known_Datum_To_WGS84_M(alat,alon,0,lat,lon,&ht,78); + + return 1; +} + + + +/* @func GPS_Math_WGS84_To_UKOSMap_C *********************************** +** +** Convert WGS84 lat/lon to Ordnance survey map code and easting and +** northing. Uses cartesian transformation +** +** @param [r] lat [double] WGS84 latitude (deg) +** @param [r] lon [double] WGS84 longitude (deg) +** @param [w] mE [double *] map easting (metres) +** @param [w] mN [double *] map northing (metres) +** @param [w] map [char *] map two letter code +** +** @return [int32] success +************************************************************************/ +int32 GPS_Math_WGS84_To_UKOSMap_C(double lat, double lon, double *mE, + double *mN, char *map) +{ + double alat; + double alon; + double aht; + double aE; + double aN; + + + GPS_Math_WGS84_To_Known_Datum_C(lat,lon,30,&alat,&alon,&aht,86); + + GPS_Math_Airy1830LatLonToNGEN(alat,alon,&aE,&aN); + + if(!GPS_Math_EN_To_UKOSNG_Map(aE,aN,mE,mN,map)) + return 0; + + return 1; +} + + + +/* @func GPS_Math_UKOSMap_To_WGS84_C *********************************** +** +** Transform UK Ordnance survey map position to WGS84 lat/lon +** Uses cartesian transformation +** +** @param [r] map [char *] map two letter code +** @param [r] mE [double] map easting (metres) +** @param [r] mN [double] map northing (metres) +** @param [w] lat [double *] WGS84 latitude (deg) +** @param [w] lon [double *] WGS84 longitude (deg) +** +** @return [int32] success +************************************************************************/ +int32 GPS_Math_UKOSMap_To_WGS84_C(char *map, double mE, double mN, + double *lat, double *lon) +{ + double E; + double N; + double alat; + double alon; + double ht; + + if(!GPS_Math_UKOSNG_Map_To_EN(map,mE,mN,&E,&N)) + return 0; + + GPS_Math_NGENToAiry1830LatLon(E,N,&alat,&alon); + + GPS_Math_Known_Datum_To_WGS84_C(alat,alon,0,lat,lon,&ht,78); + + return 1; +} + + +/* @funcstatic GPS_Math_LatLon_To_UTM_Param ***************************** +** +** Transform NAD33 +** +** @param [r] lat [double] NAD latitude (deg) +** @param [r] lon [double] NAD longitude (deg) +** @param [w] zone [int32 *] zone number +** @param [w] zc [char *] zone character +** @param [w] Mc [double *] central meridian +** @param [w] E0 [double *] false easting +** @param [w] N0 [double *] false northing +** @param [w] F0 [double *] scale factor +** +** @return [int32] success +************************************************************************/ +static int32 GPS_Math_LatLon_To_UTM_Param(double lat, double lon, int32 *zone, + char *zc, double *Mc, double *E0, + double *N0, double *F0) +{ + int32 ilon; + int32 ilat; + int32 psign; + int32 lsign; + + if(lat >= (double)84.0 || lat < (double)-80.0) + return 0; + + psign = lsign = 0; + if(lon < (double)0.0) + lsign=1; + if(lat < (double)0.0) + psign=1; + + ilon = abs((int32)lon); + ilat = abs((int32)lat); + + if(!lsign) + { + *zone = 31 + (ilon / 6); + *Mc = (double)((ilon / 6) * 6 + 3); + } + else + { + *zone = 30 - (ilon / 6); + *Mc = -(double)((ilon / 6) * 6 + 3); + } + + if(!psign) + { + *zc = 'N' + ilat / 8; + if(*zc > 'N') ++*zc; + } + else + { + *zc = 'M' - (ilat / 8); + if(*zc <= 'I') --*zc; + } + + + if(lat>=(double)56.0 && lat<(double)64.0 && lon>=(double)3.0 && + lon<(double)12.0) + { + *zone = 32; + *zc = 'V'; + *Mc = (double)9.0; + } + + if(*zc=='X' && lon>=(double)0.0 && lon<(double)42.0) + { + if(lon<(double)9.0) + { + *zone = 31; + *Mc = (double)3.0; + } + else if(lon<(double)21.0) + { + *zone = 33; + *Mc = (double)15.0; + } + else if(lon<(double)33.0) + { + *zone = 35; + *Mc = (double)27.0; + } + else + { + *zone = 37; + *Mc = (double)39.0; + } + } + + if(!psign) + *N0 = (double)0.0; + else + *N0 = (double)10000000; + + *E0 = (double)500000; + *F0 = (double)0.9996; + + return 1; +} + + + +/* @func GPS_Math_NAD83_To_UTM_EN ************************************** +** +** Transform NAD33 lat/lon to UTM zone, easting and northing +** +** @param [r] lat [double] NAD latitude (deg) +** @param [r] lon [double] NAD longitude (deg) +** @param [w] E [double *] easting (metres) +** @param [w] N [double *] northing (metres) +** @param [w] zone [int32 *] zone number +** @param [w] zc [char *] zone character +** +** @return [int32] success +************************************************************************/ +int32 GPS_Math_NAD83_To_UTM_EN(double lat, double lon, double *E, + double *N, int32 *zone, char *zc) +{ + double phi0; + double lambda0; + double N0; + double E0; + double F0; + double a; + double b; + + if(!GPS_Math_LatLon_To_UTM_Param(lat,lon,zone,zc,&lambda0,&E0, + &N0,&F0)) + return 0; + + phi0 = (double)0.0; + + a = (double) GPS_Ellipse[21].a; + b = a - (a/GPS_Ellipse[21].invf); + + GPS_Math_LatLon_To_EN(E,N,lat,lon,N0,E0,phi0,lambda0,F0,a,b); + + return 1; +} + + + +/* @func GPS_Math_WGS84_To_UTM_EN ************************************** +** +** Transform WGS84 lat/lon to UTM zone, easting and northing +** +** @param [r] lat [double] WGS84 latitude (deg) +** @param [r] lon [double] WGS84 longitude (deg) +** @param [w] E [double *] easting (metres) +** @param [w] N [double *] northing (metres) +** @param [w] zone [int32 *] zone number +** @param [w] zc [char *] zone character +** +** @return [int32] success +************************************************************************/ +int32 GPS_Math_WGS84_To_UTM_EN(double lat, double lon, double *E, + double *N, int32 *zone, char *zc) +{ + double phi; + double lambda; + double H; + + GPS_Math_WGS84_To_Known_Datum_M(lat,lon,0,&phi,&lambda,&H,77); + if(!GPS_Math_NAD83_To_UTM_EN(phi,lambda,E,N,zone,zc)) + return 0; + + return 1; +} + + + +/* @funcstatic GPS_Math_UTM_Param_To_Mc ******************************** +** +** Convert UTM zone and zone character to central meridian value. +** Also return false eastings, northings and scale factor +** +** @param [w] zone [int32] zone number +** @param [w] zc [char] zone character +** @param [w] Mc [double *] central meridian +** @param [w] E0 [double *] false easting +** @param [w] N0 [double *] false northing +** @param [w] F0 [double *] scale factor +** +** @return [int32] success +************************************************************************/ +static int32 GPS_Math_UTM_Param_To_Mc(int32 zone, char zc, double *Mc, + double *E0, double *N0, double *F0) +{ + + if(zone>60 || zone<0 || zc<'C' || zc>'X') + return 0; + + if(zone > 30) + *Mc = (double)((zone-31)*6) + (double)3.0; + else + *Mc = (double) -(((30-zone)*6)+3); + + if(zone==32 && zc=='V') + *Mc = (double)9.0; + + if(zone==31 && zc=='X') + *Mc = (double)3.0; + if(zone==33 && zc=='X') + *Mc = (double)15.0; + if(zone==35 && zc=='X') + *Mc = (double)27.0; + if(zone==37 && zc=='X') + *Mc = (double)39.0; + + if(zc>'M') + *N0 = (double)0.0; + else + *N0 = (double)10000000; + + *E0 = (double)500000; + *F0 = (double)0.9996; + + return 1; +} + + + +/* @func GPS_Math_UTM_EN_To_NAD83 ************************************** +** +** Transform UTM zone, easting and northing to NAD83 lat/lon +** +** @param [r] lat [double *] NAD latitude (deg) +** @param [r] lon [double *] NAD longitude (deg) +** @param [w] E [double] easting (metres) +** @param [w] N [double] northing (metres) +** @param [w] zone [int32] zone number +** @param [w] zc [char] zone character +** +** @return [int32] success +************************************************************************/ +int32 GPS_Math_UTM_EN_To_NAD83(double *lat, double *lon, double E, + double N, int32 zone, char zc) +{ + double phi0; + double lambda0; + double N0; + double E0; + double F0; + double a; + double b; + + if(!GPS_Math_UTM_Param_To_Mc(zone,zc,&lambda0,&E0,&N0,&F0)) + return 0; + + phi0 = (double)0.0; + + a = (double) GPS_Ellipse[21].a; + b = a - (a/GPS_Ellipse[21].invf); + + GPS_Math_EN_To_LatLon(E,N,lat,lon,N0,E0,phi0,lambda0,F0,a,b); + + return 1; +} + + + +/* @func GPS_Math_UTM_EN_To_WGS84 ************************************** +** +** Transform UTM zone, easting and northing to WGS84 lat/lon +** +** @param [w] lat [double *] WGS84 latitude (deg) +** @param [r] lon [double *] WGS84 longitude (deg) +** @param [w] E [double] easting (metres) +** @param [w] N [double] northing (metres) +** @param [w] zone [int32] zone number +** @param [w] zc [char] zone character +** +** @return [int32] success +************************************************************************/ +int32 GPS_Math_UTM_EN_To_WGS84(double *lat, double *lon, double E, + double N, int32 zone, char zc) +{ + double phi; + double lambda; + double H; + + if(!GPS_Math_UTM_EN_To_NAD83(&phi,&lambda,E,N,zone,zc)) + return 0; + + + GPS_Math_Known_Datum_To_WGS84_M(phi,lambda,0,lat,lon,&H,77); + + return 1; +} diff --git a/gpsbabel/jeeps/gpsmath.h b/gpsbabel/jeeps/gpsmath.h new file mode 100644 index 000000000..4e2249f08 --- /dev/null +++ b/gpsbabel/jeeps/gpsmath.h @@ -0,0 +1,123 @@ +#ifdef __cplusplus +extern "C" +{ +#endif + +#ifndef gpsmath_h +#define gpsmath_h + + +#include "gps.h" + +#define GPS_PI 3.141592653589 +#define GPS_FLTMIN 1.75494351E-38 +#define GPS_FLTMAX 3.402823466E+38 + + +double GPS_Math_Deg_To_Rad(double v); +double GPS_Math_Rad_To_Deg(double v); + +double GPS_Math_Metres_To_Feet(double v); +double GPS_Math_Feet_To_Metres(double v); + +int32 GPS_Math_Deg_To_Semi(double v); +double GPS_Math_Semi_To_Deg(int32 v); + +time_t GPS_Math_Utime_To_Gtime(time_t v); +time_t GPS_Math_Gtime_To_Utime(time_t v); + +void GPS_Math_Deg_To_DegMin(double v, int32 *d, double *m); +void GPS_Math_DegMin_To_Deg(int32 d, double m, double *deg); +void GPS_Math_Deg_To_DegMinSec(double v, int32 *d, int32 *m, double *s); +void GPS_Math_DegMinSec_To_Deg(int32 d, int32 m, double s, double *deg); + + +void GPS_Math_Airy1830LatLonToNGEN(double phi, double lambda, double *E, + double *N); +void GPS_Math_Airy1830M_LatLonToINGEN(double phi, double lambda, double *E, + double *N); +int32 GPS_Math_EN_To_UKOSNG_Map(double E, double N, double *mE, + double *mN, char *map); +int32 GPS_Math_UKOSNG_Map_To_EN(char *map, double mapE, double mapN, + double *E, double *N); + +void GPS_Math_LatLonH_To_XYZ(double phi, double lambda, double H, + double *x, double *y, double *z, + double a, double b); +void GPS_Math_XYZ_To_LatLonH(double *phi, double *lambda, double *H, + double x, double y, double z, + double a, double b); + +void GPS_Math_EN_To_LatLon(double E, double N, double *phi, + double *lambda, double N0, double E0, + double phi0, double lambda0, + double F0, double a, double b); +void GPS_Math_LatLon_To_EN(double *E, double *N, double phi, + double lambda, double N0, double E0, + double phi0, double lambda0, + double F0, double a, double b); + +void GPS_Math_NGENToAiry1830LatLon(double E, double N, double *phi, + double *lambda); +void GPS_Math_INGENToAiry1830MLatLon(double E, double N, double *phi, + double *lambda); + + +void GPS_Math_Airy1830LatLonH_To_XYZ(double phi, double lambda, double H, + double *x, double *y, double *z); +void GPS_Math_WGS84LatLonH_To_XYZ(double phi, double lambda, double H, + double *x, double *y, double *z); +void GPS_Math_XYZ_To_Airy1830LatLonH(double *phi, double *lambda, double *H, + double x, double y, double z); +void GPS_Math_XYZ_To_WGS84LatLonH(double *phi, double *lambda, double *H, + double x, double y, double z); + +void GPS_Math_Molodensky(double Sphi, double Slam, double SH, double Sa, + double Sif, double *Dphi, double *Dlam, + double *DH, double Da, double Dif, double dx, + double dy, double dz); +void GPS_Math_Known_Datum_To_WGS84_M(double Sphi, double Slam, double SH, + double *Dphi, double *Dlam, double *DH, + int32 n); +void GPS_Math_WGS84_To_Known_Datum_M(double Sphi, double Slam, double SH, + double *Dphi, double *Dlam, double *DH, + int32 n); +void GPS_Math_Known_Datum_To_WGS84_C(double Sphi, double Slam, double SH, + double *Dphi, double *Dlam, double *DH, + int32 n); +void GPS_Math_WGS84_To_Known_Datum_C(double Sphi, double Slam, double SH, + double *Dphi, double *Dlam, double *DH, + int32 n); + +void GPS_Math_Known_Datum_To_Known_Datum_M(double Sphi, double Slam, double SH, + double *Dphi, double *Dlam, + double *DH, int32 n1, int32 n2); +void GPS_Math_Known_Datum_To_Known_Datum_C(double Sphi, double Slam, double SH, + double *Dphi, double *Dlam, + double *DH, int32 n1, int32 n2); + +int32 GPS_Math_WGS84_To_UKOSMap_M(double lat, double lon, double *mE, + double *mN, char *map); +int32 GPS_Math_UKOSMap_To_WGS84_M(char *map, double mE, double mN, + double *lat, double *lon); +int32 GPS_Math_WGS84_To_UKOSMap_C(double lat, double lon, double *mE, + double *mN, char *map); +int32 GPS_Math_UKOSMap_To_WGS84_C(char *map, double mE, double mN, + double *lat, double *lon); + + +int32 GPS_Math_NAD83_To_UTM_EN(double lat, double lon, double *E, + double *N, int32 *zone, char *zc); +int32 GPS_Math_WGS84_To_UTM_EN(double lat, double lon, double *E, + double *N, int32 *zone, char *zc); + +int32 GPS_Math_UTM_EN_To_WGS84(double *lat, double *lon, double E, + double N, int32 zone, char zc); +int32 GPS_Math_UTM_EN_To_NAD83(double *lat, double *lon, double E, + double N, int32 zone, char zc); + +#endif + +#ifdef __cplusplus +} +#endif diff --git a/gpsbabel/jeeps/gpsmem.c b/gpsbabel/jeeps/gpsmem.c new file mode 100644 index 000000000..e2c83679c --- /dev/null +++ b/gpsbabel/jeeps/gpsmem.c @@ -0,0 +1,1459 @@ +/******************************************************************** +** @source JEEPS constructor and deconstructor functions +** +** @author Copyright (C) 1999,2000 Alan Bleasby +** @version 1.0 +** @modified December 28th 1999 Alan Bleasby. First version +** @modified June 29th 2000 Alan Bleasby. NMEA additions +** @@ +** +** This library is free software; you can redistribute it and/or +** modify it under the terms of the GNU Library General Public +** License as published by the Free Software Foundation; either +** version 2 of the License, or (at your option) any later version. +** +** This library is distributed in the hope that it will be useful, +** but WITHOUT ANY WARRANTY; without even the implied warranty of +** MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU +** Library General Public License for more details. +** +** You should have received a copy of the GNU Library General Public +** License along with this library; if not, write to the +** Free Software Foundation, Inc., 59 Temple Place - Suite 330, +** Boston, MA 02111-1307, USA. +********************************************************************/ +#include "gps.h" +#include +#include +#include +#include + + +/* @func GPS_Packet_New *********************************************** +** +** Packet constructor +** +** @return [GPS_PPacket] virgin packet +**********************************************************************/ + +GPS_PPacket GPS_Packet_New(void) +{ + GPS_PPacket ret; + + if(!(ret=(GPS_PPacket )malloc(sizeof(GPS_OPacket)))) + { + perror("malloc"); + fprintf(stderr,"GPS_Packet_New: Insufficient memory"); + fflush(stderr); + return NULL; + } + + if(!(ret->data = (UC *)malloc(MAX_GPS_PACKET_SIZE*sizeof(UC)))) + { + perror("malloc"); + fprintf(stderr,"GPS_Packet_New: Insufficient data memory"); + fflush(stderr); + return NULL; + } + + ret->dle = ret->edle = DLE; + ret->etx = ETX; + + return ret; +} + + + +/* @func GPS_Packet_Del *********************************************** +** +** Packet destructor +** +** @param [w] thys [GPS_PPacket *] packet to delete +** +** @return [void] +**********************************************************************/ + +void GPS_Packet_Del(GPS_PPacket *thys) +{ + free((void *)(*thys)->data); + free((void *)*thys); + + return; +} + + + +/* @func GPS_Pvt_New *********************************************** +** +** Pvt constructor +** +** @return [GPS_PPvt_Data] virgin pvt +**********************************************************************/ + +GPS_PPvt_Data GPS_Pvt_New(void) +{ + GPS_PPvt_Data ret; + + if(!(ret=(GPS_PPvt_Data)malloc(sizeof(GPS_OPvt_Data)))) + { + perror("malloc"); + fprintf(stderr,"GPS_Pvt_New: Insufficient memory"); + fflush(stderr); + return NULL; + } + + return ret; +} + + + +/* @func GPS_Pvt_Del *********************************************** +** +** Pvt destructor +** +** @param [w] thys [GPS_PPvt_Data *] pvt to delete +** +** @return [void] +**********************************************************************/ + +void GPS_Pvt_Del(GPS_PPvt_Data *thys) +{ + free((void *)*thys); + + return; +} + + + +/* @func GPS_Almanac_New *********************************************** +** +** Almanac constructor +** +** @return [GPS_PAlmanac] virgin almanac +**********************************************************************/ + +GPS_PAlmanac GPS_Almanac_New(void) +{ + GPS_PAlmanac ret; + + if(!(ret=(GPS_PAlmanac)malloc(sizeof(GPS_OAlmanac)))) + { + perror("malloc"); + fprintf(stderr,"GPS_Almanac_New: Insufficient memory"); + fflush(stderr); + return NULL; + } + + ret->svid=0xff; + ret->wn = -1; + ret->hlth=0xff; + + return ret; +} + + + +/* @func GPS_Almanac_Del *********************************************** +** +** Almanac destructor +** +** @param [w] thys [GPS_PAlmanac *] almanac to delete +** +** @return [void] +**********************************************************************/ + +void GPS_Almanac_Del(GPS_PAlmanac *thys) +{ + free((void *)*thys); + + return; +} + + + +/* @func GPS_Track_New *********************************************** +** +** Track constructor +** +** @return [GPS_PTrack] virgin track +**********************************************************************/ + +GPS_PTrack GPS_Track_New(void) +{ + GPS_PTrack ret; + + if(!(ret=(GPS_PTrack)malloc(sizeof(GPS_OTrack)))) + { + perror("malloc"); + fprintf(stderr,"GPS_Track_New: Insufficient memory"); + fflush(stderr); + return NULL; + } + + return ret; +} + + + +/* @func GPS_Track_Del *********************************************** +** +** Track destructor +** +** @param [w] thys [GPS_PTrack *] track to delete +** +** @return [void] +**********************************************************************/ + +void GPS_Track_Del(GPS_PTrack *thys) +{ + free((void *)*thys); + + return; +} + + + +/* @func GPS_Way_New *********************************************** +** +** Waypoint constructor +** +** @return [GPS_PWay] virgin waypoint +**********************************************************************/ + +GPS_PWay GPS_Way_New(void) +{ + GPS_PWay ret; + int32 i; + + if(!(ret=(GPS_PWay)malloc(sizeof(GPS_OWay)))) + { + perror("malloc"); + fprintf(stderr,"GPS_Way_New: Insufficient memory"); + fflush(stderr); + return NULL; + } + + /* Mark all as "unused" */ + for(i=0;i<6;++i) ret->ident[i]=' '; + for(i=0;i<2;++i) ret->cc[i]=' '; + for(i=0;i<18;++i) ret->subclass[i]=' '; + for(i=0;i<40;++i) ret->cmnt[i]=' '; + for(i=0;i<24;++i) ret->city[i]=' '; + for(i=0;i<2;++i) ret->state[i]=' '; + for(i=0;i<30;++i) ret->name[i]=' '; + for(i=0;i<18;++i) ret->rte_link_subclass[i]=' '; + for(i=0;i<256;++i) ret->wpt_ident[i]=' '; + for(i=0;i<256;++i) ret->lnk_ident[i]=' '; + for(i=0;i<256;++i) ret->rte_link_ident[i]=' '; + + ret->dst = ret->lat = ret->lon = GPS_FLTMAX; + ret->smbl = ret->dspl = ret->colour = ret->alt = ret->prot = INT_MAX; + + if(gps_waypt_type==pD108) + { + ret->dst = 0; + ret->attr = 0x60; + } + + return ret; +} + + + +/* @func GPS_Way_Del *********************************************** +** +** Waypoint destructor +** +** @param [w] thys [GPS_Pway *] waypoint to delete +** +** @return [void] +**********************************************************************/ + +void GPS_Way_Del(GPS_PWay *thys) +{ + free((void *)*thys); + + return; +} + + + + +/* @func GPS_Gsv_New *********************************************** +** +** Satellites in view constructor +** +** @return [GPS_PGsv] virgin siv +**********************************************************************/ + +GPS_PGsv GPS_Gsv_New(void) +{ + GPS_PGsv ret; + + if(!(ret=(GPS_PGsv)malloc(sizeof(GPS_OGsv)))) + return NULL; + + ret->valid = ret->inview = 0; + *ret->elevation = *ret->azimuth = *ret->strength = '\0'; + + return ret; +} + + + +/* @func GPS_Gsv_Del *********************************************** +** +** Satellites in view destructor +** +** @param [w] thys [GPS_PGsv *] siv to delete +** +** @return [void] +**********************************************************************/ + +void GPS_Gsv_Del(GPS_PGsv *thys) +{ + free((void *)*thys); + + return; +} + + + + +/* @func GPS_Rme_New *********************************************** +** +** Position error constructor +** +** @return [GPS_PRme] virgin rme +**********************************************************************/ + +GPS_PRme GPS_Rme_New(void) +{ + GPS_PRme ret; + + if(!(ret=(GPS_PRme)malloc(sizeof(GPS_ORme)))) + return NULL; + + ret->valid = 0; + ret->hpe = ret->vpe = ret->spe = (double)0.; + + return ret; +} + + + +/* @func GPS_Rme_Del *********************************************** +** +** Position error destructor +** +** @param [w] thys [GPS_PRme *] posn error to delete +** +** @return [void] +**********************************************************************/ + +void GPS_Rme_Del(GPS_PRme *thys) +{ + free((void *)*thys); + + return; +} + + + + +/* @func GPS_Wpl_New *********************************************** +** +** Waypoint constructor +** +** @return [GPS_PWpl] virgin rme +**********************************************************************/ + +GPS_PWpl GPS_Wpl_New(void) +{ + GPS_PWpl ret; + + if(!(ret=(GPS_PWpl)malloc(sizeof(GPS_OWpl)))) + return NULL; + + ret->valid = 0; + *ret->wpt = '\0'; + ret->lat = ret->lon = (double)0.; + + return ret; +} + + + +/* @func GPS_Wpl_Del *********************************************** +** +** Waypoint destructor +** +** @param [w] thys [GPS_PWpl *] waypoint to delete +** +** @return [void] +**********************************************************************/ + +void GPS_Wpl_Del(GPS_PWpl *thys) +{ + free((void *)*thys); + + return; +} + + + + +/* @func GPS_Gll_New *********************************************** +** +** Position (geographic lat/lon) constructor +** +** @return [GPS_PGll] virgin gll +**********************************************************************/ + +GPS_PGll GPS_Gll_New(void) +{ + GPS_PGll ret; + + if(!(ret=(GPS_PGll)malloc(sizeof(GPS_OGll)))) + return NULL; + + ret->valid = 0; + ret->time = (time_t)0; + ret->lat = ret->lon = (double)0.; + ret->dv='\0'; + + return ret; +} + + + +/* @func GPS_Gll_Del *********************************************** +** +** Position destructor +** +** @param [w] thys [GPS_PGll *] posn to delete +** +** @return [void] +**********************************************************************/ + +void GPS_Gll_Del(GPS_PGll *thys) +{ + free((void *)*thys); + + return; +} + + + + +/* @func GPS_Rmz_New *********************************************** +** +** Altitude constructor +** +** @return [GPS_PRmz] virgin altitude +**********************************************************************/ + +GPS_PRmz GPS_Rmz_New(void) +{ + GPS_PRmz ret; + + if(!(ret=(GPS_PRmz)malloc(sizeof(GPS_ORmz)))) + return NULL; + + ret->valid = ret->dim = ret->height = 0; + + return ret; +} + + + +/* @func GPS_Rmz_Del *********************************************** +** +** Altitude destructor +** +** @param [w] thys [GPS_PRmz *] altitude to delete +** +** @return [void] +**********************************************************************/ + +void GPS_Rmz_Del(GPS_PRmz *thys) +{ + free((void *)*thys); + + return; +} + + + + +/* @func GPS_Rmm_New *********************************************** +** +** Datum constructor +** +** @return [GPS_PRmm] virgin datum +**********************************************************************/ + +GPS_PRmm GPS_Rmm_New(void) +{ + GPS_PRmm ret; + + if(!(ret=(GPS_PRmm)malloc(sizeof(GPS_ORmm)))) + return NULL; + + ret->valid = 0; + *ret->datum = '\0'; + + return ret; +} + + + +/* @func GPS_Rmm_Del *********************************************** +** +** Datum destructor +** +** @param [w] thys [GPS_PRmm *] datum to delete +** +** @return [void] +**********************************************************************/ + +void GPS_Rmm_Del(GPS_PRmm *thys) +{ + free((void *)*thys); + + return; +} + + + + +/* @func GPS_Bod_New *********************************************** +** +** Bearing constructor +** +** @return [GPS_PBod] virgin bearing +**********************************************************************/ + +GPS_PBod GPS_Bod_New(void) +{ + GPS_PBod ret; + + if(!(ret=(GPS_PBod)malloc(sizeof(GPS_OBod)))) + return NULL; + + ret->valid = 0; + *ret->dest = *ret->start = '\0'; + ret->true = ret->mag = (double)0.; + + return ret; +} + + + +/* @func GPS_Bod_Del *********************************************** +** +** Bearing destructor +** +** @param [w] thys [GPS_PBod *] bearing to delete +** +** @return [void] +**********************************************************************/ + +void GPS_Bod_Del(GPS_PBod *thys) +{ + free((void *)*thys); + + return; +} + + + + +/* @func GPS_Rte_New *********************************************** +** +** Route (NMEA) constructor +** +** @return [GPS_PRte] virgin bearing +**********************************************************************/ + +GPS_PRte GPS_Rte_New(void) +{ + GPS_PRte ret; + + if(!(ret=(GPS_PRte)malloc(sizeof(GPS_ORte)))) + return NULL; + + ret->valid = ret->rte = 0; + ret->type = '\0'; + ret->wpts = NULL; + + return ret; +} + + + +/* @func GPS_Rte_Del *********************************************** +** +** Route (NMEA) destructor +** +** @param [w] thys [GPS_PRte *] route to delete +** +** @return [void] +**********************************************************************/ + +void GPS_Rte_Del(GPS_PRte *thys) +{ + if((*thys)->wpts) + free((void *)(*thys)->wpts); + free((void *)*thys); + + return; +} + + + + +/* @func GPS_Rmc_New *********************************************** +** +** Minimum recommended specific constructor +** +** @return [GPS_PRmc] virgin minimum +**********************************************************************/ + +GPS_PRmc GPS_Rmc_New(void) +{ + GPS_PRmc ret; + + if(!(ret=(GPS_PRmc)malloc(sizeof(GPS_ORmc)))) + return NULL; + + ret->valid = 0; + ret->time = (time_t)0; + *ret->date = ret->warn = '\0'; + ret->lat = ret->lon = ret->speed = ret->cmg = ret->magvar = + (double)0.; + + return ret; +} + + + +/* @func GPS_Rmc_Del *********************************************** +** +** Minimum recommended specific destructor +** +** @param [w] thys [GPS_PRmc *] rec min to delete +** +** @return [void] +**********************************************************************/ + +void GPS_Rmc_Del(GPS_PRmc *thys) +{ + free((void *)*thys); + + return; +} + + + + +/* @func GPS_Rmb_New *********************************************** +** +** Minimum recommended nav constructor +** +** @return [GPS_PRmb] virgin minimum nav +**********************************************************************/ + +GPS_PRmb GPS_Rmb_New(void) +{ + GPS_PRmb ret; + + if(!(ret=(GPS_PRmb)malloc(sizeof(GPS_ORmb)))) + return NULL; + + ret->valid = 0; + *ret->owpt = *ret->dwpt = ret->warn = ret->correct = ret->alarm = '\0'; + ret->cross = ret->lat = ret->lon = ret->range = ret->true = ret->velocity = + (double)0.; + + return ret; +} + + + +/* @func GPS_Rmb_Del *********************************************** +** +** Minimum recommended nav destructor +** +** @param [w] thys [GPS_PRmb *] rec min nav to delete +** +** @return [void] +**********************************************************************/ + +void GPS_Rmb_Del(GPS_PRmb *thys) +{ + free((void *)*thys); + + return; +} + + + + +/* @func GPS_Gga_New *********************************************** +** +** Fix constructor +** +** @return [GPS_PGga] virgin fix +**********************************************************************/ + +GPS_PGga GPS_Gga_New(void) +{ + GPS_PGga ret; + + if(!(ret=(GPS_PGga)malloc(sizeof(GPS_OGga)))) + return NULL; + + ret->time = (time_t)0.; + ret->valid = ret->qual = ret->nsat = ret->last = ret->dgpsid = 0; + ret->hdil = ret->lat = ret->lon = ret->alt = ret->galt = (double)0.; + + return ret; +} + + + +/* @func GPS_Gga_Del *********************************************** +** +** Fix destructor +** +** @param [w] thys [GPS_PGga *] fix to delete +** +** @return [void] +**********************************************************************/ + +void GPS_Gga_Del(GPS_PGga *thys) +{ + free((void *)*thys); + + return; +} + + + + +/* @func GPS_Gsa_New *********************************************** +** +** DOP constructor +** +** @return [GPS_PGsa] virgin DOP +**********************************************************************/ + +GPS_PGsa GPS_Gsa_New(void) +{ + GPS_PGsa ret; + + if(!(ret=(GPS_PGsa)malloc(sizeof(GPS_OGsa)))) + return NULL; + + ret->type = '\0'; + ret->valid = ret->nsat = ret->fix = 0; + ret->pdop = ret->hdop = ret->vdop = (double)0.; + + return ret; +} + + + +/* @func GPS_Gsa_Del *********************************************** +** +** DOP destructor +** +** @param [w] thys [GPS_PGsa *] DOP to delete +** +** @return [void] +**********************************************************************/ + +void GPS_Gsa_Del(GPS_PGsa *thys) +{ + free((void *)*thys); + + return; +} + + + + +/* @func GPS_Apb_New *********************************************** +** +** Autopilot B constructor +** +** @return [GPS_PApb] virgin autopilot +**********************************************************************/ + +GPS_PApb GPS_Apb_New(void) +{ + GPS_PApb ret; + + if(!(ret=(GPS_PApb)malloc(sizeof(GPS_OApb)))) + return NULL; + + ret->blink = ret->warn = ret->steer = ret->unit = ret->alarmc = + ret->alarmp = *ret->wpt = '\0'; + ret->valid = 0; + ret->edist = ret->od = ret->pd = ret->hdg = (double)0.; + + return ret; +} + + + +/* @func GPS_Apb_Del *********************************************** +** +** Autopilot destructor +** +** @param [w] thys [GPS_PApb *] autopilot to delete +** +** @return [void] +**********************************************************************/ + +void GPS_Apb_Del(GPS_PApb *thys) +{ + free((void *)*thys); + + return; +} + + + + +/* @func GPS_Bwc_New *********************************************** +** +** Waypoint bng constructor +** +** @return [GPS_PBwc] virgin waypoint bng +**********************************************************************/ + +GPS_PBwc GPS_Bwc_New(void) +{ + GPS_PBwc ret; + + if(!(ret=(GPS_PBwc)malloc(sizeof(GPS_OBwc)))) + return NULL; + + *ret->wpt = '\0'; + ret->time = (time_t)0; + ret->valid = 0; + ret->lat = ret->lon = ret->true = ret->mag = ret->dist = (double)0.; + + return ret; +} + + + +/* @func GPS_Bwc_Del *********************************************** +** +** Waypoint bearing destructor +** +** @param [w] thys [GPS_PBwc *] waypoint bearing to delete +** +** @return [void] +**********************************************************************/ + +void GPS_Bwc_Del(GPS_PBwc *thys) +{ + free((void *)*thys); + + return; +} + + + + +/* @func GPS_Bwr_New *********************************************** +** +** Waypoint bng rhumb constructor +** +** @return [GPS_PBwr] virgin waypoint bng +**********************************************************************/ + +GPS_PBwr GPS_Bwr_New(void) +{ + GPS_PBwr ret; + + if(!(ret=(GPS_PBwr)malloc(sizeof(GPS_OBwr)))) + return NULL; + + *ret->wpt = '\0'; + ret->time = (time_t)0; + ret->valid = 0; + ret->lat = ret->lon = ret->true = ret->mag = ret->dist = (double)0.; + + return ret; +} + + + +/* @func GPS_Bwr_Del *********************************************** +** +** Waypoint bearing rhumb destructor +** +** @param [w] thys [GPS_PBwr *] waypoint bearing rhumb to delete +** +** @return [void] +**********************************************************************/ + +void GPS_Bwr_Del(GPS_PBwr *thys) +{ + free((void *)*thys); + + return; +} + + + + +/* @func GPS_Dbt_New *********************************************** +** +** Depth constructor +** +** @return [GPS_PDbt] virgin depth +**********************************************************************/ + +GPS_PDbt GPS_Dbt_New(void) +{ + GPS_PDbt ret; + + if(!(ret=(GPS_PDbt)malloc(sizeof(GPS_ODbt)))) + return NULL; + + ret->valid = 0; + ret->f = ret->m = (double)0.; + + return ret; +} + + + +/* @func GPS_Dbt_Del *********************************************** +** +** Depth destructor +** +** @param [w] thys [GPS_PDbt *] depth to delete +** +** @return [void] +**********************************************************************/ + +void GPS_Dbt_Del(GPS_PDbt *thys) +{ + free((void *)*thys); + + return; +} + + + + +/* @func GPS_Hdm_New *********************************************** +** +** Magnetic heading constructor +** +** @return [GPS_PHdm] virgin hdg +**********************************************************************/ + +GPS_PHdm GPS_Hdm_New(void) +{ + GPS_PHdm ret; + + if(!(ret=(GPS_PHdm)malloc(sizeof(GPS_OHdm)))) + return NULL; + + ret->valid = 0; + ret->hdg = (double)0.; + + return ret; +} + + + +/* @func GPS_Hdm_Del *********************************************** +** +** Magnetic heading destructor +** +** @param [w] thys [GPS_PHdm *] mag hdg to delete +** +** @return [void] +**********************************************************************/ + +void GPS_Hdm_Del(GPS_PHdm *thys) +{ + free((void *)*thys); + + return; +} + + + + +/* @func GPS_Hsc_New *********************************************** +** +** Heading to steer constructor +** +** @return [GPS_PHsc] virgin hdg +**********************************************************************/ + +GPS_PHsc GPS_Hsc_New(void) +{ + GPS_PHsc ret; + + if(!(ret=(GPS_PHsc)malloc(sizeof(GPS_OHsc)))) + return NULL; + + ret->valid = 0; + ret->true = ret->mag = (double)0.; + + return ret; +} + + + +/* @func GPS_Hsc_Del *********************************************** +** +** Heading to steer destructor +** +** @param [w] thys [GPS_PHsc *] hdg to delete +** +** @return [void] +**********************************************************************/ + +void GPS_Hsc_Del(GPS_PHsc *thys) +{ + free((void *)*thys); + + return; +} + + + + +/* @func GPS_Mtw_New *********************************************** +** +** Water temp constructor +** +** @return [GPS_PMtw] virgin temp +**********************************************************************/ + +GPS_PMtw GPS_Mtw_New(void) +{ + GPS_PMtw ret; + + if(!(ret=(GPS_PMtw)malloc(sizeof(GPS_OMtw)))) + return NULL; + + ret->valid = 0; + ret->T = (double)0.; + + return ret; +} + + + +/* @func GPS_Mtw_Del *********************************************** +** +** Water temperature destructor +** +** @param [w] thys [GPS_PMtw *] water temp to delete +** +** @return [void] +**********************************************************************/ + +void GPS_Mtw_Del(GPS_PMtw *thys) +{ + free((void *)*thys); + + return; +} + + + + +/* @func GPS_R00_New *********************************************** +** +** Waypoint list constructor +** +** @return [GPS_PR00] virgin wpt list +**********************************************************************/ + +GPS_PR00 GPS_R00_New(void) +{ + GPS_PR00 ret; + + if(!(ret=(GPS_PR00)malloc(sizeof(GPS_OR00)))) + return NULL; + + ret->valid = 0; + *ret->wpts='\0'; + + return ret; +} + + + +/* @func GPS_R00_Del *********************************************** +** +** Waypoint list destructor +** +** @param [w] thys [GPS_PR00 *] waypoint list to delete +** +** @return [void] +**********************************************************************/ + +void GPS_R00_Del(GPS_PR00 *thys) +{ + free((void *)*thys); + + return; +} + + + + +/* @func GPS_Vhw_New *********************************************** +** +** Water speed constructor +** +** @return [GPS_PVhw] virgin water speed +**********************************************************************/ + +GPS_PVhw GPS_Vhw_New(void) +{ + GPS_PVhw ret; + + if(!(ret=(GPS_PVhw)malloc(sizeof(GPS_OVhw)))) + return NULL; + + ret->valid = 0; + ret->true = ret->mag = ret->wspeed = ret->speed = (double)0.; + + return ret; +} + + + +/* @func GPS_Vhw_Del *********************************************** +** +** Water speed destructor +** +** @param [w] thys [GPS_PVhw *] waypoint list to delete +** +** @return [void] +**********************************************************************/ + +void GPS_Vhw_Del(GPS_PVhw *thys) +{ + free((void *)*thys); + + return; +} + + + + +/* @func GPS_Vwr_New *********************************************** +** +** Wind constructor +** +** @return [GPS_PVwr] virgin wind +**********************************************************************/ + +GPS_PVwr GPS_Vwr_New(void) +{ + GPS_PVwr ret; + + if(!(ret=(GPS_PVwr)malloc(sizeof(GPS_OVwr)))) + return NULL; + + ret->wdir = '\0'; + ret->valid = 0; + ret->wind = ret->knots = ret->ms = ret->khr = (double)0.; + + return ret; +} + + + +/* @func GPS_Vwr_Del *********************************************** +** +** Wind destructor +** +** @param [w] thys [GPS_PVwr *] wind to delete +** +** @return [void] +**********************************************************************/ + +void GPS_Vwr_Del(GPS_PVwr *thys) +{ + free((void *)*thys); + + return; +} + + + + +/* @func GPS_Vtg_New *********************************************** +** +** Track made good constructor +** +** @return [GPS_PVtg] virgin tmg +**********************************************************************/ + +GPS_PVtg GPS_Vtg_New(void) +{ + GPS_PVtg ret; + + if(!(ret=(GPS_PVtg)malloc(sizeof(GPS_OVtg)))) + return NULL; + + ret->valid = 0; + ret->true = ret->mag = ret->knots = ret->khr = (double)0.; + + return ret; +} + + + +/* @func GPS_Vtg_Del *********************************************** +** +** Track made good destructor +** +** @param [w] thys [GPS_PVtg *] tmg to delete +** +** @return [void] +**********************************************************************/ + +void GPS_Vtg_Del(GPS_PVtg *thys) +{ + free((void *)*thys); + + return; +} + + + + +/* @func GPS_Xte_New *********************************************** +** +** Cross track error constructor +** +** @return [GPS_Xte] virgin xte +**********************************************************************/ + +GPS_PXte GPS_Xte_New(void) +{ + GPS_PXte ret; + + if(!(ret=(GPS_PXte)malloc(sizeof(GPS_OXte)))) + return NULL; + + ret->valid = 0; + ret->warn = ret->cycle = ret->steer = ret->unit = '\0'; + ret->dist = (double)0.; + + return ret; +} + + + +/* @func GPS_Xte_Del *********************************************** +** +** Cross track error destructor +** +** @param [w] thys [GPS_PXte *] xte to delete +** +** @return [void] +**********************************************************************/ + +void GPS_Xte_Del(GPS_PXte *thys) +{ + free((void *)*thys); + + return; +} + + + + +/* @func GPS_Xtr_New *********************************************** +** +** Cross track error dead constructor +** +** @return [GPS_Xtr] virgin xtr +**********************************************************************/ + +GPS_PXtr GPS_Xtr_New(void) +{ + GPS_PXtr ret; + + if(!(ret=(GPS_PXtr)malloc(sizeof(GPS_OXtr)))) + return NULL; + + ret->valid = 0; + ret->steer = ret->unit = '\0'; + ret->dist = (double)0.; + + return ret; +} + + + +/* @func GPS_Xtr_Del *********************************************** +** +** Cross track error dead destructor +** +** @param [w] thys [GPS_PXtr *] xtr to delete +** +** @return [void] +**********************************************************************/ + +void GPS_Xtr_Del(GPS_PXtr *thys) +{ + free((void *)*thys); + + return; +} + + + + +/* @func GPS_Lib_New *********************************************** +** +** Link constructor +** +** @return [GPS_Lib] virgin link +**********************************************************************/ + +GPS_PLib GPS_Lib_New(void) +{ + GPS_PLib ret; + + if(!(ret=(GPS_PLib)malloc(sizeof(GPS_OLib)))) + return NULL; + + ret->valid = 0; + ret->rqst = '\0'; + ret->freq = ret->baud = (double)0.; + + return ret; +} + + + +/* @func GPS_Lib_Del *********************************************** +** +** Link destructor +** +** @param [w] thys [GPS_PLib *] link to delete +** +** @return [void] +**********************************************************************/ + +void GPS_Lib_Del(GPS_PLib *thys) +{ + free((void *)*thys); + + return; +} + + + + +/* @func GPS_Nmea_New *********************************************** +** +** Nmea data constructor +** +** @return [GPS_PNmea] virgin nmea data +**********************************************************************/ + +GPS_PNmea GPS_Nmea_New(void) +{ + GPS_PNmea ret; + + if(!(ret=(GPS_PNmea)malloc(sizeof(GPS_ONmea)))) + return NULL; + + ret->gsv = GPS_Gsv_New(); + ret->rme = GPS_Rme_New(); + ret->gll = GPS_Gll_New(); + ret->rmz = GPS_Rmz_New(); + ret->rmm = GPS_Rmm_New(); + ret->bod = GPS_Bod_New(); + ret->rte = GPS_Rte_New(); + ret->wpl = GPS_Wpl_New(); + ret->rmc = GPS_Rmc_New(); + ret->rmb = GPS_Rmb_New(); + ret->gga = GPS_Gga_New(); + ret->gsa = GPS_Gsa_New(); + ret->apb = GPS_Apb_New(); + ret->bwc = GPS_Bwc_New(); + ret->bwr = GPS_Bwr_New(); + ret->dbt = GPS_Dbt_New(); + ret->hdm = GPS_Hdm_New(); + ret->hsc = GPS_Hsc_New(); + ret->mtw = GPS_Mtw_New(); + ret->r00 = GPS_R00_New(); + ret->vhw = GPS_Vhw_New(); + ret->vwr = GPS_Vwr_New(); + ret->vtg = GPS_Vtg_New(); + ret->xte = GPS_Xte_New(); + ret->xtr = GPS_Xtr_New(); + ret->lib = GPS_Lib_New(); + + return ret; +} + + + +/* @func GPS_Nmea_Del *********************************************** +** +** NMEA data destructor +** +** @param [w] thys [GPS_PNmea *] nmea data to delete +** +** @return [void] +**********************************************************************/ + +void GPS_Nmea_Del(GPS_PNmea *thys) +{ + + GPS_Gsv_Del(&(*thys)->gsv); + GPS_Rme_Del(&(*thys)->rme); + GPS_Gll_Del(&(*thys)->gll); + GPS_Rmz_Del(&(*thys)->rmz); + GPS_Rmm_Del(&(*thys)->rmm); + GPS_Bod_Del(&(*thys)->bod); + GPS_Rte_Del(&(*thys)->rte); + GPS_Wpl_Del(&(*thys)->wpl); + GPS_Rmc_Del(&(*thys)->rmc); + GPS_Rmb_Del(&(*thys)->rmb); + GPS_Gga_Del(&(*thys)->gga); + GPS_Gsa_Del(&(*thys)->gsa); + GPS_Apb_Del(&(*thys)->apb); + GPS_Bwc_Del(&(*thys)->bwc); + GPS_Bwr_Del(&(*thys)->bwr); + GPS_Dbt_Del(&(*thys)->dbt); + GPS_Hdm_Del(&(*thys)->hdm); + GPS_Hsc_Del(&(*thys)->hsc); + GPS_Mtw_Del(&(*thys)->mtw); + GPS_R00_Del(&(*thys)->r00); + GPS_Vhw_Del(&(*thys)->vhw); + GPS_Vwr_Del(&(*thys)->vwr); + GPS_Vtg_Del(&(*thys)->vtg); + GPS_Xte_Del(&(*thys)->xte); + GPS_Xtr_Del(&(*thys)->xtr); + GPS_Lib_Del(&(*thys)->lib); + + free((void *)*thys); + + return; +} diff --git a/gpsbabel/jeeps/gpsmem.h b/gpsbabel/jeeps/gpsmem.h new file mode 100644 index 000000000..8a52fad2e --- /dev/null +++ b/gpsbabel/jeeps/gpsmem.h @@ -0,0 +1,88 @@ +#ifdef __cplusplus +extern "C" +{ +#endif + +#ifndef gpsmem_h +#define gpsmem_h + + +#include "gps.h" + +GPS_PPacket GPS_Packet_New(void); +void GPS_Packet_Del(GPS_PPacket *thys); +GPS_PPvt_Data GPS_Pvt_New(void); +void GPS_Pvt_Del(GPS_PPvt_Data *thys); +GPS_PAlmanac GPS_Almanac_New(void); +void GPS_Almanac_Del(GPS_PAlmanac *thys); +GPS_PTrack GPS_Track_New(void); +void GPS_Track_Del(GPS_PTrack *thys); +GPS_PWay GPS_Way_New(void); +void GPS_Way_Del(GPS_PWay *thys); + + +/* + * NMEA Section + */ +GPS_PGsv GPS_Gsv_New(void); +void GPS_Gsv_Del(GPS_PGsv *thys); +GPS_PRme GPS_Rme_New(void); +void GPS_Rme_Del(GPS_PRme *thys); +GPS_PGll GPS_Gll_New(void); +void GPS_Gll_Del(GPS_PGll *thys); +GPS_PRmz GPS_Rmz_New(void); +void GPS_Rmz_Del(GPS_PRmz *thys); +GPS_PRmm GPS_Rmm_New(void); +void GPS_Rmm_Del(GPS_PRmm *thys); +GPS_PBod GPS_Bod_New(void); +void GPS_Bod_Del(GPS_PBod *thys); +GPS_PRte GPS_Rte_New(void); +void GPS_Rte_Del(GPS_PRte *thys); +GPS_PRmc GPS_Rmc_New(void); +void GPS_Rmc_Del(GPS_PRmc *thys); +GPS_PRmb GPS_Rmb_New(void); +void GPS_Rmb_Del(GPS_PRmb *thys); +GPS_PGga GPS_Gga_New(void); +void GPS_Gga_Del(GPS_PGga *thys); +GPS_PGsa GPS_Gsa_New(void); +void GPS_Gsa_Del(GPS_PGsa *thys); +GPS_PApb GPS_Apb_New(void); +void GPS_Apb_Del(GPS_PApb *thys); +GPS_PBwc GPS_Bwc_New(void); +void GPS_Bwc_Del(GPS_PBwc *thys); +GPS_PBwr GPS_Bwr_New(void); +void GPS_Bwr_Del(GPS_PBwr *thys); +GPS_PDbt GPS_Dbt_New(void); +void GPS_Dbt_Del(GPS_PDbt *thys); +GPS_PHdm GPS_Hdm_New(void); +void GPS_Hdm_Del(GPS_PHdm *thys); +GPS_PHsc GPS_Hsc_New(void); +void GPS_Hsc_Del(GPS_PHsc *thys); +GPS_PMtw GPS_Mtw_New(void); +void GPS_Mtw_Del(GPS_PMtw *thys); +GPS_PR00 GPS_R00_New(void); +void GPS_R00_Del(GPS_PR00 *thys); +GPS_PVhw GPS_Vhw_New(void); +void GPS_Vhw_Del(GPS_PVhw *thys); +GPS_PVwr GPS_Vwr_New(void); +void GPS_Vwr_Del(GPS_PVwr *thys); +GPS_PVtg GPS_Vtg_New(void); +void GPS_Vtg_Del(GPS_PVtg *thys); +GPS_PXte GPS_Xte_New(void); +void GPS_Xte_Del(GPS_PXte *thys); +GPS_PXtr GPS_Xtr_New(void); +void GPS_Xtr_Del(GPS_PXtr *thys); +GPS_PLib GPS_Lib_New(void); +void GPS_Lib_Del(GPS_PLib *thys); +GPS_PWpl GPS_Wpl_New(void); +void GPS_Wpl_Del(GPS_PWpl *thys); +GPS_PNmea GPS_Nmea_New(void); +void GPS_Nmea_Del(GPS_PNmea *thys); + + + +#endif + +#ifdef __cplusplus +} +#endif diff --git a/gpsbabel/jeeps/gpsnmea.h b/gpsbabel/jeeps/gpsnmea.h new file mode 100644 index 000000000..163cb0cdb --- /dev/null +++ b/gpsbabel/jeeps/gpsnmea.h @@ -0,0 +1,312 @@ +#ifdef __cplusplus +extern "C" +{ +#endif + +#ifndef gpsnmea_h +#define gpsnmea_h + + +#include "gps.h" + + +typedef struct GPS_SGsv +{ + int32 inview; + int32 prn[32]; + int32 elevation[32]; + int32 azimuth[32]; + int32 strength[32]; + int32 valid; +} GPS_OGsv,*GPS_PGsv; + +typedef struct GPS_SRme +{ + double hpe; + double vpe; + double spe; + int32 valid; +} GPS_ORme,*GPS_PRme; + +typedef struct GPS_SGll +{ + double lat; + double lon; + time_t time; + char dv; + int32 valid; +} GPS_OGll,*GPS_PGll; + +typedef struct GPS_SRmz +{ + int32 height; + int32 dim; + int32 valid; +} GPS_ORmz,*GPS_PRmz; + +typedef struct GPS_SRmm +{ + char datum[83]; + int32 valid; +} GPS_ORmm,*GPS_PRmm; + +typedef struct GPS_SBod +{ + double true; + double mag; + char dest[83]; + char start[83]; + int32 valid; +} GPS_OBod,*GPS_PBod; + +typedef struct GPS_SRte +{ + char type; + int32 rte; + char *wpts; + int32 valid; +} GPS_ORte,*GPS_PRte; + +typedef struct GPS_SWpl +{ + double lat; + double lon; + char wpt[83]; + int32 valid; +} GPS_OWpl,*GPS_PWpl; + +typedef struct GPS_SRmc +{ + time_t time; + char warn; + double lat; + double lon; + double speed; + double cmg; + char date[83]; + double magvar; + int32 valid; +} GPS_ORmc,*GPS_PRmc; + +typedef struct GPS_SRmb +{ + char warn; + double cross; + char correct; + char owpt[83]; + char dwpt[83]; + double lat; + double lon; + double range; + double true; + double velocity; + char alarm; + int32 valid; +} GPS_ORmb,*GPS_PRmb; + +typedef struct GPS_SGga +{ + time_t time; + double lat; + double lon; + int32 qual; + int32 nsat; + double hdil; + double alt; + double galt; + int32 last; + int32 dgpsid; + int32 valid; +} GPS_OGga,*GPS_PGga; + +typedef struct GPS_SGsa +{ + char type; + int32 fix; + int32 nsat; + int32 prn[12]; + double pdop; + double hdop; + double vdop; + int32 valid; +} GPS_OGsa,*GPS_PGsa; + +typedef struct GPS_SApb +{ + char blink; + char warn; + double edist; + char steer; + char unit; + char alarmc; + char alarmp; + double od; + char wpt[83]; + double pd; + double hdg; + int32 valid; +} GPS_OApb,*GPS_PApb; + +typedef struct GPS_SBwc +{ + time_t time; + double lat; + double lon; + double true; + double mag; + double dist; + char wpt[83]; + int32 valid; +} GPS_OBwc,*GPS_PBwc; + +typedef struct GPS_SBwr +{ + time_t time; + double lat; + double lon; + double true; + double mag; + double dist; + char wpt[83]; + int32 valid; +} GPS_OBwr,*GPS_PBwr; + +typedef struct GPS_SDbt +{ + double f; + double m; + int32 valid; +} GPS_ODbt,*GPS_PDbt; + +typedef struct GPS_SHdm +{ + double hdg; + int32 valid; +} GPS_OHdm,*GPS_PHdm; + +typedef struct GPS_SHsc +{ + double true; + double mag; + int32 valid; +} GPS_OHsc,*GPS_PHsc; + +typedef struct GPS_SMtw +{ + double T; + int32 valid; +} GPS_OMtw,*GPS_PMtw; + +typedef struct GPS_SR00 +{ + char wpts[83]; + int32 valid; +} GPS_OR00,*GPS_PR00; + +typedef struct GPS_SVhw +{ + double true; + double mag; + double wspeed; + double speed; + int32 valid; +} GPS_OVhw,*GPS_PVhw; + +typedef struct GPS_SVwr +{ + double wind; + char wdir; + double knots; + double ms; + double khr; + int32 valid; +} GPS_OVwr,*GPS_PVwr; + +typedef struct GPS_SVtg +{ + double true; + double mag; + double knots; + double khr; + int32 valid; +} GPS_OVtg,*GPS_PVtg; + +typedef struct GPS_SXte +{ + char warn; + char cycle; + double dist; + char steer; + char unit; + int32 valid; +} GPS_OXte,*GPS_PXte; + +typedef struct GPS_SXtr +{ + double dist; + char steer; + char unit; + int32 valid; +} GPS_OXtr,*GPS_PXtr; + +typedef struct GPS_SLib +{ + double freq; + double baud; + char rqst; + int32 valid; +} GPS_OLib,*GPS_PLib; + + +typedef struct GPS_SNmea +{ + GPS_PGsv gsv; + GPS_PRme rme; + GPS_PGll gll; + GPS_PRmz rmz; + GPS_PRmm rmm; + GPS_PBod bod; + GPS_PRte rte; + GPS_PWpl wpl; + GPS_PRmc rmc; + GPS_PRmb rmb; + GPS_PGga gga; + GPS_PGsa gsa; + GPS_PApb apb; + GPS_PBwc bwc; + GPS_PBwr bwr; + GPS_PDbt dbt; + GPS_PHdm hdm; + GPS_PHsc hsc; + GPS_PMtw mtw; + GPS_PR00 r00; + GPS_PVhw vhw; + GPS_PVwr vwr; + GPS_PVtg vtg; + GPS_PXte xte; + GPS_PXtr xtr; + GPS_PLib lib; +} GPS_ONmea,*GPS_PNmea; + + + + + + +extern int32 gps_fd; /* FD for serial port access [NMEA] */ +extern GPS_PNmea gps_nmea; /* Internal nmea data repository */ + + + +void GPS_NMEA_Add_Checksum(char *s); +int32 GPS_NMEA_Line_Check(const char *s); +int32 GPS_NMEA_Load(int32 fd); +int32 GPS_NMEA_Init(const char *s); +void GPS_NMEA_Exit(void); +int32 GPS_NMEA_Send(const char *s, int32 flag); + +#endif + +#ifdef __cplusplus +} +#endif diff --git a/gpsbabel/jeeps/gpsnmeafmt.h b/gpsbabel/jeeps/gpsnmeafmt.h new file mode 100644 index 000000000..91d38041c --- /dev/null +++ b/gpsbabel/jeeps/gpsnmeafmt.h @@ -0,0 +1,44 @@ +#ifdef __cplusplus +extern "C" +{ +#endif + +#ifndef gpsnmeafmt_h +#define gpsnmeafmt_h + + +#include "gps.h" + +int32 GPS_NMEA_Apb_Scan(const char *s, GPS_PApb *thys); +int32 GPS_NMEA_Bod_Scan(const char *s, GPS_PBod *thys); +int32 GPS_NMEA_Bwc_Scan(const char *s, GPS_PBwc *thys); +int32 GPS_NMEA_Bwr_Scan(const char *s, GPS_PBwr *thys); +int32 GPS_NMEA_Dbt_Scan(const char *s, GPS_PDbt *thys); +int32 GPS_NMEA_Gga_Scan(const char *s, GPS_PGga *thys); +int32 GPS_NMEA_Gll_Scan(const char *s, GPS_PGll *thys); +int32 GPS_NMEA_Gsa_Scan(const char *s, GPS_PGsa *thys); +int32 GPS_NMEA_Gsv_Scan(const char *s, GPS_PGsv *thys); +int32 GPS_NMEA_Hdm_Scan(const char *s, GPS_PHdm *thys); +int32 GPS_NMEA_Hsc_Scan(const char *s, GPS_PHsc *thys); +int32 GPS_NMEA_Mtw_Scan(const char *s, GPS_PMtw *thys); +int32 GPS_NMEA_R00_Scan(const char *s, GPS_PR00 *thys); +int32 GPS_NMEA_Rmb_Scan(const char *s, GPS_PRmb *thys); +int32 GPS_NMEA_Rmc_Scan(const char *s, GPS_PRmc *thys); +int32 GPS_NMEA_Rte_Scan(const char *s, GPS_PRte *thys); +int32 GPS_NMEA_Vhw_Scan(const char *s, GPS_PVhw *thys); +int32 GPS_NMEA_Vwr_Scan(const char *s, GPS_PVwr *thys); +int32 GPS_NMEA_Vtg_Scan(const char *s, GPS_PVtg *thys); +int32 GPS_NMEA_Wpl_Scan(const char *s, GPS_PWpl *thys); +int32 GPS_NMEA_Xte_Scan(const char *s, GPS_PXte *thys); +int32 GPS_NMEA_Xtr_Scan(const char *s, GPS_PXtr *thys); +int32 GPS_NMEA_Rme_Scan(const char *s, GPS_PRme *thys); +int32 GPS_NMEA_Rmz_Scan(const char *s, GPS_PRmz *thys); +int32 GPS_NMEA_Rmm_Scan(const char *s, GPS_PRmm *thys); +int32 GPS_NMEA_Lib_Scan(const char *s, GPS_PLib *thys); + + +#endif + +#ifdef __cplusplus +} +#endif diff --git a/gpsbabel/jeeps/gpsnmeaget.h b/gpsbabel/jeeps/gpsnmeaget.h new file mode 100644 index 000000000..17cba4801 --- /dev/null +++ b/gpsbabel/jeeps/gpsnmeaget.h @@ -0,0 +1,43 @@ +#ifdef __cplusplus +extern "C" +{ +#endif + +#ifndef gpsnmearead_h +#define gpsnmearead_h + + +#include "gps.h" + +int32 GPS_NMEA_Get_Apb(GPS_PApb *thys); +int32 GPS_NMEA_Get_Bod(GPS_PBod *thys); +int32 GPS_NMEA_Get_Bwc(GPS_PBwc *thys); +int32 GPS_NMEA_Get_Bwr(GPS_PBwr *thys); +int32 GPS_NMEA_Get_Dbt(GPS_PDbt *thys); +int32 GPS_NMEA_Get_Gga(GPS_PGga *thys); +int32 GPS_NMEA_Get_Gll(GPS_PGll *thys); +int32 GPS_NMEA_Get_Gsa(GPS_PGsa *thys); +int32 GPS_NMEA_Get_Gsv(GPS_PGsv *thys); +int32 GPS_NMEA_Get_Hdm(GPS_PHdm *thys); +int32 GPS_NMEA_Get_Hsc(GPS_PHsc *thys); +int32 GPS_NMEA_Get_Mtw(GPS_PMtw *thys); +int32 GPS_NMEA_Get_R00(GPS_PR00 *thys); +int32 GPS_NMEA_Get_Rmb(GPS_PRmb *thys); +int32 GPS_NMEA_Get_Rmc(GPS_PRmc *thys); +int32 GPS_NMEA_Get_Rte(GPS_PRte *thys); +int32 GPS_NMEA_Get_Vhw(GPS_PVhw *thys); +int32 GPS_NMEA_Get_Vwr(GPS_PVwr *thys); +int32 GPS_NMEA_Get_Vtg(GPS_PVtg *thys); +int32 GPS_NMEA_Get_Wpl(GPS_PWpl *thys); +int32 GPS_NMEA_Get_Xte(GPS_PXte *thys); +int32 GPS_NMEA_Get_Xtr(GPS_PXtr *thys); +int32 GPS_NMEA_Get_Rme(GPS_PRme *thys); +int32 GPS_NMEA_Get_Rmz(GPS_PRmz *thys); +int32 GPS_NMEA_Get_Rmm(GPS_PRmm *thys); +int32 GPS_NMEA_Get_Lib(GPS_PLib *thys); + +#endif + +#ifdef __cplusplus +} +#endif diff --git a/gpsbabel/jeeps/gpsport.h b/gpsbabel/jeeps/gpsport.h new file mode 100644 index 000000000..040efd2e7 --- /dev/null +++ b/gpsbabel/jeeps/gpsport.h @@ -0,0 +1,16 @@ +/* + * For portability any '32' type must be 32 bits + * and '16' type must be 16 bits + */ +typedef unsigned char UC; +typedef short int16; +typedef unsigned short uint16; +typedef uint16 US; + +#if defined(__alpha) +typedef int int32; +typedef unsigned int uint32; +#else +typedef long int32; +typedef unsigned long uint32; +#endif diff --git a/gpsbabel/jeeps/gpsproj.c b/gpsbabel/jeeps/gpsproj.c new file mode 100644 index 000000000..c2634b370 --- /dev/null +++ b/gpsbabel/jeeps/gpsproj.c @@ -0,0 +1,4485 @@ +/******************************************************************** +** @source JEEPS projection functions +** +** @author Copyright (C) 1999 Alan Bleasby +** @version 1.0 +** @modified Feb 04 2000 Alan Bleasby. First version +** @@ +** +** This library is free software; you can redistribute it and/or +** modify it under the terms of the GNU Library General Public +** License as published by the Free Software Foundation; either +** version 2 of the License, or (at your option) any later version. +** +** This library is distributed in the hope that it will be useful, +** but WITHOUT ANY WARRANTY; without even the implied warranty of +** MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU +** Library General Public License for more details. +** +** You should have received a copy of the GNU Library General Public +** License along with this library; if not, write to the +** Free Software Foundation, Inc., 59 Temple Place - Suite 330, +** Boston, MA 02111-1307, USA. +********************************************************************/ +#include "gps.h" +#include +#include + + +/* @func GPS_Math_Albers_LatLon_To_EN ********************************** +** +** Convert latitude and longitude to Albers projection easting and +** northing +** +** @param [r] phi [double] latitude (deg) +** @param [r] lambda [double] longitude (deg) +** @param [w] E [double *] easting (metre) +** @param [w] N [double *] northing (metre) +** @param [r] phi1 [double] standard latitude (parallel) 1 (deg) +** @param [r] phi2 [double] standard latitude (parallel) 2 (deg) +** @param [r] phi0 [double] latitude of origin (deg) +** @param [r] M0 [double] central meridian (deg) +** @param [r] E0 [double] false easting +** @param [r] N0 [double] false northing +** @param [r] a [double] semi-major axis +** @param [r] b [double] semi-minor axis +** +** @return [void] +************************************************************************/ +void GPS_Math_Albers_LatLon_To_EN(double phi, double lambda, double *E, + double *N, double phi1, double phi2, + double phi0, double M0, double E0, + double N0, double a, double b) + +{ + double dlambda; + double phis; + double phic; + double e; + double esq; + double esqs; + double omesqs2; + + double a2; + double b2; + double q; + double q0; + double q1; + double q2; + double m1; + double m2; + double n; + double phi0s; + double phi1s; + double phi1c; + double phi2s; + double phi2c; + double ess; + double om0; + double m1sq; + double C; + double nq; + double nq0; + double rho; + double rho0; + double theta; + + phi = GPS_Math_Deg_To_Rad(phi); + phi0 = GPS_Math_Deg_To_Rad(phi0); + phi1 = GPS_Math_Deg_To_Rad(phi1); + phi2 = GPS_Math_Deg_To_Rad(phi2); + lambda = GPS_Math_Deg_To_Rad(lambda); + M0 = GPS_Math_Deg_To_Rad(M0); + + dlambda = lambda - M0; + if(dlambda > GPS_PI) + dlambda -= ((double)2.0 * GPS_PI); + if(dlambda < -GPS_PI) + dlambda += ((double)2.0 * GPS_PI); + + phis = sin(phi); + phic = cos(phi); + + a2 = a*a; + b2 = b*b; + esq = (a2-b2)/a2; + e = pow(esq,(double)0.5); + + + phi0s = sin(phi0); + ess = e * phi0s; + om0 = ((double)1.0 - ess*ess); + q0 = ((double)1.0 - esq) * (phi0s / om0-((double)1.0/(e+e)) * + log(((double)1.0-ess)/((double)1.0+ess))); + phi1s = sin(phi1); + phi1c = cos(phi1); + ess = e * phi1s; + om0 = ((double)1.0 - ess*ess); + m1 = phi1c/pow(om0,(double)0.5); + q1 = ((double)1.0 - esq) * (phi1s / om0-((double)1.0/(e+e)) * + log(((double)1.0-ess)/((double)1.0+ess))); + + m1sq = m1*m1; + if(fabs(phi1-phi2)>1.0e-10) + { + phi2s = sin(phi2); + phi2c = cos(phi2); + ess = e * phi2s; + om0 = ((double)1.0 - ess*ess); + m2 = phi2c/pow(om0,(double)0.5); + q2 = ((double)1.0 - esq) * (phi2s / om0-((double)1.0/(e+e)) * + log(((double)1.0-ess)/((double)1.0+ess))); + n = (m1sq - m2*m2) / (q2-q1); + } + else + n = phi1s; + + C = m1sq + n*q1; + nq0 = n * q0; + if(C < nq0) + rho0 = (double)0.; + else + rho0 = (a/n) * pow(C-nq0,(double)0.5); + + + esqs = e * phis; + omesqs2 = ((double)1.0 - esqs*esqs); + q = ((double)1.0 - esq) * (phis / omesqs2-((double)1.0/(e+e)) * + log(((double)1.0-esqs)/((double)1.0+esqs))); + nq = n*q; + if(C1.0e-10) + { + phi2s = sin(phi2); + phi2c = cos(phi2); + ess = e * phi2s; + om0 = ((double)1.0 - ess*ess); + m2 = phi2c/pow(om0,(double)0.5); + q2 = ((double)1.0 - esq) * (phi2s / om0-((double)1.0/(e+e)) * + log(((double)1.0-ess)/((double)1.0+ess))); + n = (m1sq - m2*m2) / (q2-q1); + } + else + n = phi1s; + + C = m1sq + n*q1; + nq0 = n * q0; + if(C < nq0) + rho0 = (double)0.; + else + rho0 = (a/n) * pow(C-nq0,(double)0.5); + + + dphi = (double) 1.0; + theta = (double) 0.0; + tol = (double) 4.85e-10; + po2 = (double)GPS_PI / (double)2.0; + + dy = N-N0; + dx = E-E0; + rhom = rho0-dy; + rho = pow(dx*dx+rhom*rhom,(double)0.5); + + if(n<0.0) + { + rho *= (double)-1.0; + dx *= (double)-1.0; + dy *= (double)-1.0; + rhom *= (double)-1.0; + } + + if(rho) + theta = atan2(dx,rhom); + rhon = rho*n; + q = (C - (rhon*rhon) / a2) / n; + qc = (double)1.0 - ((double)1.0 / (e+e)) * + log(((double)1.0-e)/((double)1.0+e)); + if(fabs(fabs(qc)-fabs(q))>1.9e-6) + { + qd2 = q/(double)2.0; + if(qd2>1.0) + *phi = po2; + else if(qd2<-1.0) + *phi = -po2; + else + { + lat = asin(qd2); + if(e<1.0e-10) + *phi = lat; + else + { + while(fabs(dphi)>tol) + { + phis = sin(lat); + ess = e*phis; + om0 = ((double)1.0 - ess*ess); + dphi = (om0*om0) / ((double)2.0*cos(lat))* + (q/((double)1.0-esq) - phis / om0 + + (log(((double)1.0-ess)/((double)1.0+ess)) / + (e+e))); + lat += dphi; + } + *phi = lat; + } + + if(*phi > po2) + *phi = po2; + else if(*phi<-po2) + *phi = -po2; + } + } + else + { + if(q>=0.0) + *phi = po2; + else + *phi = -po2; + } + + *lambda = M0 + theta / n; + if(*lambda > GPS_PI) + *lambda -= GPS_PI * (double)2.0; + if(*lambda < -GPS_PI) + *lambda += GPS_PI * (double)2.0; + if(*lambda>GPS_PI) + *lambda = GPS_PI; + else if(*lambda<-GPS_PI) + *lambda = -GPS_PI; + + *phi = GPS_Math_Rad_To_Deg(*phi); + *lambda = GPS_Math_Rad_To_Deg(*lambda); + + return; +} + + + +/* @func GPS_Math_LambertCC_LatLon_To_EN ********************************** +** +** Convert latitude and longitude to Lambert Conformal Conic projection +** easting and northing +** +** @param [r] phi [double] latitude (deg) +** @param [r] lambda [double] longitude (deg) +** @param [w] E [double *] easting (metre) +** @param [w] N [double *] northing (metre) +** @param [r] phi1 [double] standard latitude (parallel) 1 (deg) +** @param [r] phi2 [double] standard latitude (parallel) 2 (deg) +** @param [r] phi0 [double] latitude of origin (deg) +** @param [r] M0 [double] central meridian (deg) +** @param [r] E0 [double] false easting +** @param [r] N0 [double] false northing +** @param [r] a [double] semi-major axis +** @param [r] b [double] semi-minor axis +** +** @return [void] +************************************************************************/ +void GPS_Math_LambertCC_LatLon_To_EN(double phi, double lambda, double *E, + double *N, double phi1, double phi2, + double phi0, double M0, double E0, + double N0, double a, double b) + +{ + double po2; + double po4; + double a2; + double b2; + double phi0s; + double e; + double esq; + double ed2; + double ess; + double t0; + double t1; + double t2; + double m1; + double m2; + double phi1s; + double phi1c; + double phi2s; + double phi2c; + double n; + double F; + double Fa; + double rho; + double rho0; + double phis; + double t; + double theta; + double dphi; + + phi = GPS_Math_Deg_To_Rad(phi); + phi0 = GPS_Math_Deg_To_Rad(phi0); + phi1 = GPS_Math_Deg_To_Rad(phi1); + phi2 = GPS_Math_Deg_To_Rad(phi2); + lambda = GPS_Math_Deg_To_Rad(lambda); + M0 = GPS_Math_Deg_To_Rad(M0); + + + po2 = (double)GPS_PI / (double)2.0; + po4 = (double)GPS_PI / (double)4.0; + a2 = a*a; + b2 = b*b; + esq = (a2-b2)/a2; + e = pow(esq,(double)0.5); + ed2 = e / (double)2.0; + + phi0s = sin(phi0); + ess = e * phi0s; + t0 = tan(po4-phi0/(double)2.0) / pow(((double)1.0-ess) / + ((double)1.0+ess),ed2); + + + phi1s = sin(phi1); + phi1c = cos(phi1); + ess = e * phi1s; + m1 = phi1c / pow(((double)1.0-ess*ess),(double)0.5); + t1 = tan(po4-phi1/(double)2.0) / pow(((double)1.0-ess) / + ((double)1.0+ess),ed2); + + if(fabs(phi1-phi2)>1.0e-10) + { + phi2s = sin(phi2); + phi2c = cos(phi2); + ess = e * phi2s; + m2 = phi2c / pow(((double)1.0-ess*ess),(double)0.5); + t2 = tan(po4-phi2/(double)2.0) / pow(((double)1.0-ess) / + ((double)1.0+ess),ed2); + n = log(m1/m2) / log(t1/t2); + } + else + n = phi1s; + + F = m1 / (n*pow(t1,n)); + Fa = F*a; + + rho0 = pow(t0,n) * Fa; + + if(fabs(fabs(phi)-po2)>1.0e-10) + { + phis = sin(phi); + ess = e * phis; + t = tan(po4-phi/(double)2.0) / pow(((double)1.0-ess) / + ((double)1.0+ess),ed2); + rho = pow(t,n) * Fa; + } + else + { + if((phi*n)<=(double)0.0) + return; + rho = (double)0.0; + } + + dphi = lambda - M0; + if(dphi>GPS_PI) + dphi -= (double)GPS_PI * (double)2.0; + if(dphi<-GPS_PI) + dphi += (double)GPS_PI * (double)2.0; + theta = dphi*n; + + *E = rho * sin(theta) + E0; + *N = rho0 - rho * cos(theta) + N0; + + return; +} + + + + +/* @func GPS_Math_LambertCC_EN_To_LatLon ********************************** +** +** Convert Lambert Conformal Conic easting and northing to latitude and +** longitude +** +** @param [r] E [double] easting (metre) +** @param [r] N [double] northing (metre) +** @param [w] phi [double *] latitude (deg) +** @param [w] lambda [double *] longitude (deg) +** @param [r] phi1 [double] standard latitude (parallel) 1 (deg) +** @param [r] phi2 [double] standard latitude (parallel) 2 (deg) +** @param [r] phi0 [double] latitude of origin (deg) +** @param [r] M0 [double] central meridian (deg) +** @param [r] E0 [double] false easting +** @param [r] N0 [double] false northing +** @param [r] a [double] semi-major axis +** @param [r] b [double] semi-minor axis +** +** @return [void] +************************************************************************/ +void GPS_Math_LambertCC_EN_To_LatLon(double E, double N, double *phi, + double *lambda, double phi1, double phi2, + double phi0, double M0, double E0, + double N0, double a, double b) +{ + double po2; + double po4; + double a2; + double b2; + double phi0s; + double e; + double esq; + double ed2; + double ess; + double t0; + double t1; + double t2; + double m1; + double m2; + double phi1s; + double phi1c; + double phi2s; + double phi2c; + double n; + double F; + double Fa; + double rho; + double rho0; + double phis; + double t; + double theta; + + double dx; + double dy; + double rhom; + double lat; + double tlat; + double tol; + + + + phi0 = GPS_Math_Deg_To_Rad(phi0); + phi1 = GPS_Math_Deg_To_Rad(phi1); + phi2 = GPS_Math_Deg_To_Rad(phi2); + M0 = GPS_Math_Deg_To_Rad(M0); + + + po2 = (double)GPS_PI / (double)2.0; + po4 = (double)GPS_PI / (double)4.0; + a2 = a*a; + b2 = b*b; + esq = (a2-b2)/a2; + e = pow(esq,(double)0.5); + ed2 = e / (double)2.0; + + phi0s = sin(phi0); + ess = e * phi0s; + t0 = tan(po4-phi0/(double)2.0) / pow(((double)1.0-ess) / + ((double)1.0+ess),ed2); + + + phi1s = sin(phi1); + phi1c = cos(phi1); + ess = e * phi1s; + m1 = phi1c / pow(((double)1.0-ess*ess),(double)0.5); + t1 = tan(po4-phi1/(double)2.0) / pow(((double)1.0-ess) / + ((double)1.0+ess),ed2); + + if(fabs(phi1-phi2)>1.0e-10) + { + phi2s = sin(phi2); + phi2c = cos(phi2); + ess = e * phi2s; + m2 = phi2c / pow(((double)1.0-ess*ess),(double)0.5); + t2 = tan(po4-phi2/(double)2.0) / pow(((double)1.0-ess) / + ((double)1.0+ess),ed2); + n = log(m1/m2) / log(t1/t2); + } + else + n = phi1s; + + F = m1 / (n*pow(t1,n)); + Fa = F*a; + + rho0 = pow(t0,n) * Fa; + + tlat = theta = (double)0.0; + tol = (double)4.85e-10; + + dx = E - E0; + dy = N - N0; + rhom = rho0 - dy; + rho = pow(dx*dx + rhom*rhom,(double)0.5); + + if(n<0.0) + { + rhom *= (double)-1.0; + dy *= (double)-1.0; + dx *= (double)-1.0; + rho *= (double)-1.0; + } + + if(rho) + { + theta = atan2(dx,rhom); + t = pow(rho/Fa,(double)1.0/n); + lat = po2 - (double)2.0*atan(t); + while(fabs(lat-tlat)>tol) + { + tlat = lat; + phis = sin(lat); + ess = e * phis; + lat = po2 - (double)2.0 * atan(t*pow(((double)1.0-ess) / + ((double)1.0+ess), + e / (double)2.0)); + } + *phi = lat; + *lambda = theta/n + M0; + + if(*phi>po2) + *phi=po2; + else if(*phi<-po2) + *phi=-po2; + if(*lambda>GPS_PI) + *lambda -= (double)GPS_PI * (double)2.0; + if(*lambda<-GPS_PI) + *lambda += (double)GPS_PI * (double)2.0; + + if(*lambda>GPS_PI) + *lambda = GPS_PI; + else if(*lambda<-GPS_PI) + *lambda = -GPS_PI; + } + else + { + if(n>0.0) + *phi = po2; + else + *phi = -po2; + *lambda = M0; + } + + *lambda = GPS_Math_Rad_To_Deg(*lambda); + *phi = GPS_Math_Rad_To_Deg(*phi); + + return; +} + + + + +/* @func GPS_Math_Miller_LatLon_To_EN ********************************** +** +** Convert latitude and longitude to Miller Cylindrical projection easting and +** northing +** +** @param [r] phi [double] latitude (deg) +** @param [r] lambda [double] longitude (deg) +** @param [w] E [double *] easting (metre) +** @param [w] N [double *] northing (metre) +** @param [r] M0 [double] central meridian (deg) +** @param [r] E0 [double] false easting +** @param [r] N0 [double] false northing +** @param [r] a [double] semi-major axis +** @param [r] b [double] semi-minor axis +** +** @return [void] +************************************************************************/ +void GPS_Math_Miller_LatLon_To_EN(double phi, double lambda, double *E, + double *N, double M0, double E0, + double N0, double a, double b) +{ + double a2; + double b2; + double R; + double e2; + double e4; + double e6; + double p2; + double po2; + double phis; + double dlam; + + + phi = GPS_Math_Deg_To_Rad(phi); + lambda = GPS_Math_Deg_To_Rad(lambda); + M0 = GPS_Math_Deg_To_Rad(M0); + + po2 = (double)GPS_PI / (double)2.0; + p2 = (double)GPS_PI * (double)2.0; + a2 = a*a; + b2 = b*b; + e2 = (a2-b2)/a2; + e4 = e2*e2; + e6 = e4*e2; + + R = a*((double)1.0-e2/(double)6.0-(double)17.0*e4/(double)360.0- + (double)67.0*e6/(double)3024.0); + + if(M0>GPS_PI) + M0 -= p2; + + phis = sin((double)0.8 * phi); + + dlam = lambda - M0; + if(dlam>GPS_PI) + dlam-=p2; + if(dlam<-GPS_PI) + dlam+=p2; + + *E = R*dlam+E0; + *N = (R/(double)1.6) * log(((double)1.0+phis) / ((double)1.0-phis)) + N0; + + return; +} + + + + +/* @func GPS_Math_Miller_EN_To_LatLon ********************************** +** +** Convert latitude and longitude to Miller Cylindrical projection easting and +** northing +** +** @param [r] E [double] easting (metre) +** @param [r] N [double] northing (metre) +** @param [w] phi [double *] latitude (deg) +** @param [w] lambda [double *] longitude (deg) +** @param [r] M0 [double] central meridian (deg) +** @param [r] E0 [double] false easting +** @param [r] N0 [double] false northing +** @param [r] a [double] semi-major axis +** @param [r] b [double] semi-minor axis +** +** @return [void] +************************************************************************/ +void GPS_Math_Miller_EN_To_LatLon(double E, double N, double *phi, + double *lambda, double M0, double E0, + double N0, double a, double b) +{ + double a2; + double b2; + double R; + double e; + double e2; + double e4; + double e6; + double p2; + double po2; + double dx; + double dy; + + dx = E - E0; + dy = N - N0; + + M0 = GPS_Math_Deg_To_Rad(M0); + + po2 = (double)GPS_PI / (double)2.0; + p2 = (double)GPS_PI * (double)2.0; + a2 = a*a; + b2 = b*b; + e2 = (a2-b2)/a2; + e4 = e2*e2; + e6 = e4*e2; + e = pow(e2,(double)0.5); + + R = a*((double)1.0-e2/(double)6.0-(double)17.0*e4/(double)360.0- + (double)67.0*e6/(double)3024.0); + if(M0>GPS_PI) + M0 -= p2; + + *phi = atan(sinh((double)0.8*dy/R)) / (double)0.8; + *lambda = M0+dx/R; + + if(*phi>po2) + *phi=po2; + else if (*phi<-po2) + *phi=-po2; + + if(*lambda>GPS_PI) + *lambda-=p2; + if(*lambda<-GPS_PI) + *lambda+=p2; + + if(*lambda>GPS_PI) + *lambda=GPS_PI; + else if(*lambda<-GPS_PI) + *lambda=-GPS_PI; + + *lambda = GPS_Math_Rad_To_Deg(*lambda); + *phi = GPS_Math_Rad_To_Deg(*phi); + + return; +} + + + + +/* @func GPS_Math_Bonne_LatLon_To_EN ********************************** +** +** Convert latitude and longitude to Bonne pseudoconic equal area projection +** easting and northing +** +** @param [r] phi [double] latitude (deg) +** @param [r] lambda [double] longitude (deg) +** @param [w] E [double *] easting (metre) +** @param [w] N [double *] northing (metre) +** @param [r] phi0 [double] latitude of origin (deg) +** @param [r] M0 [double] central meridian (deg) +** @param [r] E0 [double] false easting +** @param [r] N0 [double] false northing +** @param [r] a [double] semi-major axis +** @param [r] b [double] semi-minor axis +** +** @return [void] +************************************************************************/ +void GPS_Math_Bonne_LatLon_To_EN(double phi, double lambda, double *E, + double *N, double phi0, double M0, double E0, + double N0, double a, double b) +{ + double p2; + double po2; + double a2; + double b2; + double e2; + double e4; + double e6; + double M1; + double m1; + double c0; + double c1; + double c2; + double c3; + double j; + double te4; + double E1; + double E2; + double E3; + double E4; + double x; + double phi0s; + double lat; + double phi0c; + double phi0s2; + double phi0s4; + double phi0s6; + double as; + + double phis; + double phic; + double phis2; + double phis4; + double phis6; + double dlam; + double mm; + double MM; + double rho; + double EE; + double tol; + + + lambda = GPS_Math_Deg_To_Rad(lambda); + phi = GPS_Math_Deg_To_Rad(phi); + phi0 = GPS_Math_Deg_To_Rad(phi0); + M0 = GPS_Math_Deg_To_Rad(M0); + + phi0s = sin(phi0); + p2 = (double)GPS_PI * (double)2.0; + po2 = (double)GPS_PI / (double)2.0; + if(M0>GPS_PI) + M0 -= p2; + a2 = a*a; + b2 = b*b; + e2 = (a2-b2)/a2; + e4 = e2*e2; + e6 = e2*e4; + + j = (double)45.0*e6/(double)1024.0; + te4 = (double)3.0 * e4; + c0 = (double)1.0-e2/(double)4.0-te4/(double)64.0-(double)5.0*e6/ + (double)256.0; + c1 = (double)3.0*e2/(double)8.0+te4/(double)32.0+j; + c2 = (double)15.0*e4/(double)256.0+j; + c3 = (double)35.0*e6/(double)3072.0; + + phi0c = cos(phi0); + m1 = phi0c/ pow(((double)1.0-e2*phi0s*phi0s),(double)0.5); + lat = c0 * phi0; + + phi0s2 = c1 * sin((double)2.0*phi0); + phi0s4 = c2 * sin((double)4.0*phi0); + phi0s6 = c3 * sin((double)6.0*phi0); + M1 = a*(lat-phi0s2+phi0s4-phi0s6); + + x = pow((double)1.0-e2,(double)0.5); + E1 = ((double)1.0-x) / ((double)1.0+x); + E2 = E1*E1; + E3 = E2*E1; + E4 = E3*E1; + + if(!phi0s) + as = (double)0.0; + else + as = a*m1/phi0s; + + + dlam = lambda - M0; + if(dlam>GPS_PI) + dlam -= p2; + if(dlam<-GPS_PI) + dlam += p2; + + phis = sin(phi); + phic = cos(phi); + + tol = (double)0.0001; + if(!(phi-phi0) && (((po2-tol)GPS_PI) + M0 -= p2; + a2 = a*a; + b2 = b*b; + e2 = (a2-b2)/a2; + e4 = e2*e2; + e6 = e2*e4; + + j = (double)45.0*e6/(double)1024.0; + te4 = (double)3.0 * e4; + c0 = (double)1.0-e2/(double)4.0-te4/(double)64.0-(double)5.0*e6/ + (double)256.0; + c1 = (double)3.0*e2/(double)8.0+te4/(double)32.0+j; + c2 = (double)15.0*e4/(double)256.0+j; + c3 = (double)35.0*e6/(double)3072.0; + + phi0c = cos(phi0); + m1 = phi0c/ pow(((double)1.0-e2*phi0s*phi0s),(double)0.5); + lat = c0 * phi0; + + phi0s2 = c1 * sin((double)2.0*phi0); + phi0s4 = c2 * sin((double)4.0*phi0); + phi0s6 = c3 * sin((double)6.0*phi0); + M1 = a*(lat-phi0s2+phi0s4-phi0s6); + + x = pow((double)1.0-e2,(double)0.5); + E1 = ((double)1.0-x) / ((double)1.0+x); + E2 = E1*E1; + E3 = E2*E1; + E4 = E3*E1; + A0 = (double)3.0*E1/(double)2.0-(double)27.0*E3/(double)32.0; + A1 = (double)21.0*E2/(double)16.0-(double)55.0*E4/(double)32.0; + A2 = (double)151.0*E3/(double)96.0; + A3 = (double)1097.0*E4/(double)512.0; + if(!phi0s) + as = (double)0.0; + else + as = a*m1/phi0s; + + + dx = E - E0; + dy = N - N0; + asdy = as - dy; + rho = pow(dx*dx+asdy*asdy,(double)0.5); + if(phi0<(double)0.0) + rho=-rho; + MM = as+M1-rho; + + mu = MM / (a*c0); + smu2 = A0 * sin((double)2.0*mu); + smu4 = A1 * sin((double)4.0*mu); + smu6 = A2 * sin((double)6.0*mu); + smu8 = A3 * sin((double)8.0*mu); + *phi = mu+smu2+smu4+smu6+smu8; + + tol = (double)0.00001; + if(((po2-tol)po2) + *phi = po2; + else if(*phi<-po2) + *phi = -po2; + + if(*lambda>GPS_PI) + *lambda -= p2; + if(*lambda<-GPS_PI) + *lambda += p2; + + if(*lambda>GPS_PI) + *lambda = GPS_PI; + else if(*lambda<-GPS_PI) + *lambda=-GPS_PI; + + *lambda = GPS_Math_Rad_To_Deg(*lambda); + *phi = GPS_Math_Rad_To_Deg(*phi); + + return; +} + + + + +/* @func GPS_Math_Cassini_LatLon_To_EN ********************************** +** +** Convert latitude and longitude to Cassini transverse cylindrical projection +** easting and northing +** +** @param [r] phi [double] latitude (deg) +** @param [r] lambda [double] longitude (deg) +** @param [w] E [double *] easting (metre) +** @param [w] N [double *] northing (metre) +** @param [r] phi0 [double] latitude of origin (deg) +** @param [r] M0 [double] central meridian (deg) +** @param [r] E0 [double] false easting +** @param [r] N0 [double] false northing +** @param [r] a [double] semi-major axis +** @param [r] b [double] semi-minor axis +** +** @return [void] +************************************************************************/ +void GPS_Math_Cassini_LatLon_To_EN(double phi, double lambda, double *E, + double *N, double phi0, double M0, + double E0, double N0, double a, double b) +{ + double p2; + double po2; + double a2; + double b2; + double e2; + double e4; + double e6; + double AM0; + double c0; + double c1; + double c2; + double c3; + double om0; + double A0; + double A1; + double A2; + double A3; + double j; + double te4; + double phi0s2; + double phi0s4; + double phi0s6; + double lat; + double x; + double E1; + double E2; + double E3; + double E4; + + double phis; + double phic; + double phit; + double phis2; + double phis4; + double phis6; + double RD; + double dlam; + double NN; + double TT; + double WW; + double WW2; + double WW3; + double WW4; + double WW5; + double CC; + double MM; + + + lambda = GPS_Math_Deg_To_Rad(lambda); + phi0 = GPS_Math_Deg_To_Rad(phi0); + phi = GPS_Math_Deg_To_Rad(phi); + M0 = GPS_Math_Deg_To_Rad(M0); + + + p2 = (double)GPS_PI * (double)2.; + po2 = (double)GPS_PI / (double)2.; + + a2 = a*a; + b2 = b*b; + e2 = (a2-b2)/a2; + e4 = e2*e2; + e6 = e2*e4; + + te4 = (double)3.0 * e4; + j = (double)45. * e6 / (double)1024.; + c0 = (double)1.0-e2/(double)4.-te4/(double)64.-(double)5.*e6/(double)256.; + c1 = (double)3.*e2/(double)8.+te4/(double)32.+j; + c2 = (double)15.*e4/(double)256.+j; + c3 = (double)35.*e6/(double)3072.; + + lat = c0*phi0; + phi0s2 = c1 * sin((double)2.*phi0); + phi0s4 = c2 * sin((double)4.*phi0); + phi0s6 = c3 * sin((double)6.*phi0); + AM0 = a * (lat-phi0s2+phi0s4-phi0s6); + + om0 = (double)1.0 - e2; + x = pow(om0,(double)0.5); + E1 = ((double)1.0 - x) / ((double)1.0 + x); + E2 = E1*E1; + E3 = E1*E2; + E4 = E1*E3; + A0 = (double)3.*E1/(double)2.-(double)27.*E3/(double)32.; + A1 = (double)21.*E2/(double)16.-(double)55.*E4/(double)32.; + A2 = (double)151.*E3/(double)96.; + A3 = (double)1097.*E4/(double)512.; + + + dlam = lambda - M0; + if(dlam>GPS_PI) + dlam -= p2; + if(dlam<-GPS_PI) + dlam += p2; + + phis = sin(phi); + phic = cos(phi); + phit = tan(phi); + RD = pow((double)1.-e2*phis*phis,(double).5); + NN = a/RD; + TT = phit*phit; + WW = dlam*phic; + WW2 = WW*WW; + WW3 = WW*WW2; + WW4 = WW*WW3; + WW5 = WW*WW4; + CC = e2*phic*phic/om0; + lat = c0*phi; + phis2 = c1 * sin((double)2.*phi); + phis4 = c2 * sin((double)4.*phi); + phis6 = c3 * sin((double)6.*phi); + MM = a * (lat-phis2+phis4-phis6); + + *E = NN*(WW-(TT*WW3/(double)6.)-((double)8.-TT+(double)8.*CC)* + (TT*WW5/(double)120.)) + E0; + *N = MM-AM0+NN*phit*((WW2/(double)2.)+((double)5.-TT+(double)6.*CC) * + WW4/(double)24.) + N0; + return; +} + + + + +/* @func GPS_Math_Cassini_EN_To_LatLon ********************************** +** +** Convert Cassini transverse cylindrical easting and northing projection +** to latitude and longitude +** +** @param [r] E [double] easting (metre) +** @param [r] N [double] northing (metre) +** @param [w] phi [double *] latitude (deg) +** @param [w] lambda [double *] longitude (deg) +** @param [r] phi0 [double] latitude of origin (deg) +** @param [r] M0 [double] central meridian (deg) +** @param [r] E0 [double] false easting +** @param [r] N0 [double] false northing +** @param [r] a [double] semi-major axis +** @param [r] b [double] semi-minor axis +** +** @return [void] +************************************************************************/ +void GPS_Math_Cassini_EN_To_LatLon(double E, double N, double *phi, + double *lambda, double phi0, double M0, + double E0, double N0, double a, double b) + +{ + double p2; + double po2; + double a2; + double b2; + double e2; + double e4; + double e6; + double AM0; + double c0; + double c1; + double c2; + double c3; + double om0; + double A0; + double A1; + double A2; + double A3; + double j; + double te4; + double phi0s2; + double phi0s4; + double phi0s6; + double lat; + double x; + double E1; + double E2; + double E3; + double E4; + + double dx; + double dy; + double mu; + double mus2; + double mus4; + double mus6; + double mus8; + double M1; + double phi1; + double phi1s; + double phi1c; + double phi1t; + double T; + double T1; + double N1; + double R1; + double RD; + double DD; + double D2; + double D3; + double D4; + double D5; + double tol; + + M0 = GPS_Math_Deg_To_Rad(M0); + phi0 = GPS_Math_Deg_To_Rad(phi0); + + p2 = (double)GPS_PI * (double)2.; + po2 = (double)GPS_PI / (double)2.; + + a2 = a*a; + b2 = b*b; + e2 = (a2-b2)/a2; + e4 = e2*e2; + e6 = e2*e4; + + te4 = (double)3.0 * e4; + j = (double)45. * e6 / (double)1024.; + c0 = (double)1.0-e2/(double)4.-te4/(double)64.-(double)5.*e6/(double)256.; + c1 = (double)3.*e2/(double)8.+te4/(double)32.+j; + c2 = (double)15.*e4/(double)256.+j; + c3 = (double)35.*e6/(double)3072.; + + lat = c0*phi0; + phi0s2 = c1 * sin((double)2.*phi0); + phi0s4 = c2 * sin((double)4.*phi0); + phi0s6 = c3 * sin((double)6.*phi0); + AM0 = a * (lat-phi0s2+phi0s4-phi0s6); + + om0 = (double)1.0 - e2; + x = pow(om0,(double)0.5); + E1 = ((double)1.0 - x) / ((double)1.0 + x); + E2 = E1*E1; + E3 = E1*E2; + E4 = E1*E3; + A0 = (double)3.*E1/(double)2.-(double)27.*E3/(double)32.; + A1 = (double)21.*E2/(double)16.-(double)55.*E4/(double)32.; + A2 = (double)151.*E3/(double)96.; + A3 = (double)1097.*E4/(double)512.; + + + + tol = (double)1.e-5; + + dx = E - E0; + dy = N - N0; + M1 = AM0 + dy; + mu = M1 / (a*c0); + mus2 = A0 * sin((double)2.*mu); + mus4 = A1 * sin((double)4.*mu); + mus6 = A2 * sin((double)6.*mu); + mus8 = A3 * sin((double)8.*mu); + phi1 = mu + mus2 + mus4 + mus6 + mus8; + + if((((po2-tol)po2) + *phi=po2; + else if(*phi<-po2) + *phi=-po2; + + if(*lambda>GPS_PI) + *lambda -= p2; + if(*lambda<-GPS_PI) + *lambda += p2; + + if(*lambda>GPS_PI) + *lambda=GPS_PI; + else if(*lambda<-GPS_PI) + *lambda=-GPS_PI; + } + + *lambda = GPS_Math_Rad_To_Deg(*lambda); + *phi = GPS_Math_Rad_To_Deg(*phi); + + return; +} + + + + +/* @func GPS_Math_Cylea_LatLon_To_EN ********************************** +** +** Convert latitude and longitude to Cylindrical equal area projection +** easting and northing +** +** @param [r] phi [double] latitude (deg) +** @param [r] lambda [double] longitude (deg) +** @param [w] E [double *] easting (metre) +** @param [w] N [double *] northing (metre) +** @param [r] phi0 [double] latitude of origin (deg) +** @param [r] M0 [double] central meridian (deg) +** @param [r] E0 [double] false easting +** @param [r] N0 [double] false northing +** @param [r] a [double] semi-major axis +** @param [r] b [double] semi-minor axis +** +** @return [void] +************************************************************************/ +void GPS_Math_Cylea_LatLon_To_EN(double phi, double lambda, double *E, + double *N, double phi0, double M0, + double E0, double N0, double a, double b) +{ + double a2; + double b2; + double e; + double e2; + double e4; + double e6; + double k0; + double ak0; + double k2; + double c0; + double c1; + double c2; + double p2; + double po2; + double phi0s; + double phi0c; + + double dlam; + double qq; + double x; + double phis; + + lambda = GPS_Math_Deg_To_Rad(lambda); + phi0 = GPS_Math_Deg_To_Rad(phi0); + phi = GPS_Math_Deg_To_Rad(phi); + M0 = GPS_Math_Deg_To_Rad(M0); + + p2 = (double)GPS_PI * (double)2.; + po2 = (double)GPS_PI / (double)2.; + + if(M0>GPS_PI) + M0-=p2; + + a2 = a*a; + b2 = b*b; + e2 = (a2-b2)/a2; + e4 = e2*e2; + e6 = e2*e4; + e = pow(e2,(double).5); + c0 = e2/(double)3.+(double)31.*e4/(double)180.+(double)517.* + e6/(double)5040.; + c1 = (double)23.*e4/(double)360.+(double)251.*e6/(double)3780.; + c2 = (double)761.*e6/(double)45360.; + + phi0s = sin(phi0); + phi0c = cos(phi0); + k0 = phi0c / pow((double)1.-e2*phi0s*phi0s,(double).5); + ak0 = a*k0; + k2 = k0 * (double)2.; + + dlam = lambda - M0; + if(dlam>GPS_PI) + dlam-=p2; + if(dlam<-GPS_PI) + dlam+=p2; + + phis = sin(phi); + x = e * phis; + qq = ((double)1.-e2)*(phis/((double)1.-x*x)-((double)1./((double)2.*e))* + log(((double)1.-x)/((double)1.+x))); + *E = ak0 * dlam + E0; + *N = a * qq / k2 + N0; + + return; +} + + + + +/* @func GPS_Math_Cylea_EN_To_LatLon ********************************** +** +** Convert Cylindrical equal area easting and northing projection +** to latitude and longitude +** +** @param [r] E [double] easting (metre) +** @param [r] N [double] northing (metre) +** @param [w] phi [double *] latitude (deg) +** @param [w] lambda [double *] longitude (deg) +** @param [r] phi0 [double] latitude of origin (deg) +** @param [r] M0 [double] central meridian (deg) +** @param [r] E0 [double] false easting +** @param [r] N0 [double] false northing +** @param [r] a [double] semi-major axis +** @param [r] b [double] semi-minor axis +** +** @return [void] +************************************************************************/ +void GPS_Math_Cylea_EN_To_LatLon(double E, double N, double *phi, + double *lambda, double phi0, double M0, + double E0, double N0, double a, double b) + +{ + double a2; + double b2; + double e; + double e2; + double e4; + double e6; + double k0; + double ak0; + double k2; + double c0; + double c1; + double c2; + double p2; + double po2; + double phi0s; + double phi0c; + + double dx; + double dy; + double qp; + double bt; + double phis; + double i; + double x; + double bs2; + double bs4; + double bs6; + + + phi0 = GPS_Math_Deg_To_Rad(phi0); + M0 = GPS_Math_Deg_To_Rad(M0); + + p2 = (double)GPS_PI * (double)2.; + po2 = (double)GPS_PI / (double)2.; + + if(M0>GPS_PI) + M0-=p2; + + a2 = a*a; + b2 = b*b; + e2 = (a2-b2)/a2; + e4 = e2*e2; + e6 = e2*e4; + e = pow(e2,(double).5); + c0 = e2/(double)3.+(double)31.*e4/(double)180.+(double)517.* + e6/(double)5040.; + c1 = (double)23.*e4/(double)360.+(double)251.*e6/(double)3780.; + c2 = (double)761.*e6/(double)45360.; + + phi0s = sin(phi0); + phi0c = cos(phi0); + k0 = phi0c / pow((double)1.-e2*phi0s*phi0s,(double).5); + ak0 = a*k0; + k2 = k0 * (double)2.; + + dx = E - E0; + dy = N - N0; + phis = sin(po2); + x = e*phis; + qp = ((double)1.-e2)*(phis/((double)1.-x*x)-((double)1./((double)2.*e))* + log(((double)1.-x)/((double)1.+x))); + i = k2*dy/(a*qp); + if(i>(double)1.) + i=(double)1.; + else if(i<(double)-1.) + i=(double)-1.; + bt = asin(i); + bs2 = c0 * sin((double)2.*bt); + bs4 = c1 * sin((double)4.*bt); + bs6 = c2 * sin((double)6.*bt); + + *phi = bt+bs2+bs4+bs6; + *lambda = M0 + dx/ak0; + + if(*phi>po2) + *phi=po2; + else if(*phi<-po2) + *phi=-po2; + + if(*lambda>GPS_PI) + *lambda -= p2; + if(*lambda<-GPS_PI) + *lambda += p2; + + if(*lambda>GPS_PI) + *lambda=GPS_PI; + else if(*lambda<-GPS_PI) + *lambda=-GPS_PI; + + *lambda = GPS_Math_Rad_To_Deg(*lambda); + *phi = GPS_Math_Rad_To_Deg(*phi); + + return; +} + + + + +/* @func GPS_Math_EckertIV_LatLon_To_EN ********************************** +** +** Convert latitude and longitude to Eckert IV equal area elliptical +** pseudocylindrical projection easting and northing +** +** @param [r] phi [double] latitude (deg) +** @param [r] lambda [double] longitude (deg) +** @param [w] E [double *] easting (metre) +** @param [w] N [double *] northing (metre) +** @param [r] M0 [double] central meridian (deg) +** @param [r] E0 [double] false easting +** @param [r] N0 [double] false northing +** @param [r] a [double] semi-major axis +** @param [r] b [double] semi-minor axis +** +** @return [void] +************************************************************************/ +void GPS_Math_EckertIV_LatLon_To_EN(double phi, double lambda, double *E, + double *N, double M0, double E0, double N0, + double a, double b) +{ + double a2; + double b2; + double e2; + double e4; + double e6; + double Ra0; + double Ra1; + double po2; + double p2; + + double Ra; + + double phis; + double theta; + double dtheta; + double thetas; + double thetac; + double n; + double dlam; + double tol; + + + lambda = GPS_Math_Deg_To_Rad(lambda); + phi = GPS_Math_Deg_To_Rad(phi); + M0 = GPS_Math_Deg_To_Rad(M0); + + p2 = (double)GPS_PI * (double)2.; + po2 = (double)GPS_PI / (double)2.; + + if(M0>GPS_PI) + M0-=p2; + + + a2 = a*a; + b2 = b*b; + e2 = (a2-b2) / a2; + e4 = e2*e2; + e6 = e2*e4; + Ra = a*((double)1.-e2/(double)6.-(double)17.*e4/(double)360.- + (double)67.*e6/(double)3024.); + Ra0 = Ra * (double)0.4222382; + Ra1 = Ra * (double)1.3265004; + + theta = phi / (double)2.; + dtheta = (double)1.; + tol = (double)4.85e-10; + phis = sin(phi); + + dlam = lambda - M0; + if(dlam>GPS_PI) + dlam -= p2; + if(dlam<-GPS_PI) + dlam += p2; + + while(fabs(dtheta)>tol) + { + thetas = sin(theta); + thetac = cos(theta); + n = theta+thetas*thetac+(double)2.*thetas; + dtheta = -(n-((double)2.+po2)*phis) / + ((double)2.*thetac*((double)1.+thetac)); + theta += dtheta; + } + + *E = Ra0*dlam*((double)1.+cos(theta))+E0; + *N = Ra1*sin(theta)+N0; + + return; +} + + + + +/* @func GPS_Math_EckertIV_EN_To_LatLon ********************************** +** +** Convert Eckert IV equal area elliptical pseudocylindrical projection +** easting and northing to latitude and longitude +** +** @param [r] E [double] easting (metre) +** @param [r] N [double] northing (metre) +** @param [w] phi [double *] latitude (deg) +** @param [w] lambda [double *] longitude (deg) +** @param [r] M0 [double] central meridian (deg) +** @param [r] E0 [double] false easting +** @param [r] N0 [double] false northing +** @param [r] a [double] semi-major axis +** @param [r] b [double] semi-minor axis +** +** @return [void] +************************************************************************/ +void GPS_Math_EckertIV_EN_To_LatLon(double E, double N, double *phi, + double *lambda, double M0, double E0, + double N0, double a, double b) +{ + double a2; + double b2; + double e2; + double e4; + double e6; + double Ra0; + double Ra1; + double po2; + double p2; + + double Ra; + double theta; + double thetas; + double thetac; + double n; + double dx; + double dy; + double i; + + + M0 = GPS_Math_Deg_To_Rad(M0); + + p2 = (double)GPS_PI * (double)2.; + po2 = (double)GPS_PI / (double)2.; + + if(M0>GPS_PI) + M0-=p2; + + + a2 = a*a; + b2 = b*b; + e2 = (a2-b2) / a2; + e4 = e2*e2; + e6 = e2*e4; + Ra = a*((double)1.-e2/(double)6.-(double)17.*e4/(double)360.- + (double)67.*e6/(double)3024.); + Ra0 = Ra * (double)0.4222382; + Ra1 = Ra * (double)1.3265004; + + dx = E - E0; + dy = N - N0; + i = dy/Ra1; + if(i>(double)1.) + i=(double)1.; + else if(i<(double)-1.) + i=(double)-1.; + + theta = asin(i); + thetas = sin(theta); + thetac = cos(theta); + n = theta+thetas*thetac+(double)2.*thetas; + + *phi = asin(n/((double)2. + po2)); + *lambda = M0 + dx / (Ra0*((double)1.+thetac)); + + if(*phi>po2) + *phi=po2; + else if(*phi<-po2) + *phi=-po2; + + if(*lambda>GPS_PI) + *lambda -= p2; + if(*lambda<-GPS_PI) + *lambda += p2; + + if(*lambda>GPS_PI) + *lambda=GPS_PI; + else if(*lambda<-GPS_PI) + *lambda=-GPS_PI; + + *lambda = GPS_Math_Rad_To_Deg(*lambda); + *phi = GPS_Math_Rad_To_Deg(*phi); + + return; +} + + + + + +/* @func GPS_Math_EckertVI_LatLon_To_EN ********************************** +** +** Convert latitude and longitude to Eckert VI equal area +** pseudocylindrical projection easting and northing +** +** @param [r] phi [double] latitude (deg) +** @param [r] lambda [double] longitude (deg) +** @param [w] E [double *] easting (metre) +** @param [w] N [double *] northing (metre) +** @param [r] M0 [double] central meridian (deg) +** @param [r] E0 [double] false easting +** @param [r] N0 [double] false northing +** @param [r] a [double] semi-major axis +** @param [r] b [double] semi-minor axis +** +** @return [void] +************************************************************************/ +void GPS_Math_EckertVI_LatLon_To_EN(double phi, double lambda, double *E, + double *N, double M0, double E0, double N0, + double a, double b) +{ + double a2; + double b2; + double e2; + double e4; + double e6; + double Ra; + double Rsq; + double IRa; + double po2; + double p2; + + double phis; + double theta; + double dtheta; + double dlam; + double tol; + + + lambda = GPS_Math_Deg_To_Rad(lambda); + phi = GPS_Math_Deg_To_Rad(phi); + M0 = GPS_Math_Deg_To_Rad(M0); + + p2 = (double)GPS_PI * (double)2.; + po2 = (double)GPS_PI / (double)2.; + + if(M0>GPS_PI) + M0-=p2; + + + a2 = a*a; + b2 = b*b; + e2 = (a2-b2) / a2; + e4 = e2*e2; + e6 = e2*e4; + Ra = a*((double)1.-e2/(double)6.-(double)17.*e4/(double)360.- + (double)67.*e6/(double)3024.); + Rsq = Ra/pow((double)2.+GPS_PI,(double).5); + IRa = (double)1./Rsq; + + phis = sin(phi); + theta = phi; + dtheta = (double)1.; + tol = (double)4.85e-10; + + dlam = lambda - M0; + if(dlam>GPS_PI) + dlam -= p2; + if(dlam<-GPS_PI) + dlam += p2; + + while(fabs(dtheta)>tol) + { + dtheta = -(theta+sin(theta)-((double)1.+po2)*phis) / + ((double)1.+cos(theta)); + theta += dtheta; + } + + *E = Rsq*dlam*((double)1.+cos(theta))+E0; + *N = (double)2.*Rsq*theta+N0; + + return; +} + + + + +/* @func GPS_Math_EckertVI_EN_To_LatLon ********************************** +** +** Convert Eckert VI equal area pseudocylindrical projection +** easting and northing to latitude and longitude +** +** @param [r] E [double] easting (metre) +** @param [r] N [double] northing (metre) +** @param [w] phi [double *] latitude (deg) +** @param [w] lambda [double *] longitude (deg) +** @param [r] M0 [double] central meridian (deg) +** @param [r] E0 [double] false easting +** @param [r] N0 [double] false northing +** @param [r] a [double] semi-major axis +** @param [r] b [double] semi-minor axis +** +** @return [void] +************************************************************************/ +void GPS_Math_EckertVI_EN_To_LatLon(double E, double N, double *phi, + double *lambda, double M0, double E0, + double N0, double a, double b) +{ + double a2; + double b2; + double e2; + double e4; + double e6; + double Rsq; + double IRa; + double po2; + double p2; + + double Ra; + double theta; + double dx; + double dy; + double i; + + + M0 = GPS_Math_Deg_To_Rad(M0); + + p2 = (double)GPS_PI * (double)2.; + po2 = (double)GPS_PI / (double)2.; + + if(M0>GPS_PI) + M0-=p2; + + + a2 = a*a; + b2 = b*b; + e2 = (a2-b2) / a2; + e4 = e2*e2; + e6 = e2*e4; + Ra = a*((double)1.-e2/(double)6.-(double)17.*e4/(double)360.- + (double)67.*e6/(double)3024.); + Rsq = Ra/pow((double)2.+GPS_PI,(double).5); + IRa = (double)1./Rsq; + + + dx = E - E0; + dy = N - N0; + theta = IRa * dy / (double)2.; + i = (theta+sin(theta)) / ((double)1.+po2); + if(i>(double)1.) + *phi = po2; + else if(i<(double)-1.) + *phi = -po2; + else + *phi= asin(i); + *lambda = M0 + IRa * dx / ((double)1.+cos(theta)); + + if(*phi>po2) + *phi=po2; + else if(*phi<-po2) + *phi=-po2; + + if(*lambda>GPS_PI) + *lambda -= p2; + if(*lambda<-GPS_PI) + *lambda += p2; + + if(*lambda>GPS_PI) + *lambda=GPS_PI; + else if(*lambda<-GPS_PI) + *lambda=-GPS_PI; + + *lambda = GPS_Math_Rad_To_Deg(*lambda); + *phi = GPS_Math_Rad_To_Deg(*phi); + + return; +} + + + + + +/* @func GPS_Math_Cyled_LatLon_To_EN ********************************** +** +** Convert latitude and longitude to cylindrical equidistant projection +** easting and northing +** +** @param [r] phi [double] latitude (deg) +** @param [r] lambda [double] longitude (deg) +** @param [w] E [double *] easting (metre) +** @param [w] N [double *] northing (metre) +** @param [r] phi0 [double] latitude of origin (deg) +** @param [r] M0 [double] central meridian (deg) +** @param [r] E0 [double] false easting +** @param [r] N0 [double] false northing +** @param [r] a [double] semi-major axis +** @param [r] b [double] semi-minor axis +** +** @return [void] +************************************************************************/ +void GPS_Math_Cyled_LatLon_To_EN(double phi, double lambda, double *E, + double *N, double phi0, double M0, double E0, + double N0, double a, double b) +{ + double p2; + double po2; + double a2; + double b2; + double e2; + double e4; + double e6; + double Ra; + double Rac; + double phi0c; + + double dlam; + + lambda = GPS_Math_Deg_To_Rad(lambda); + phi = GPS_Math_Deg_To_Rad(phi); + phi0 = GPS_Math_Deg_To_Rad(phi0); + M0 = GPS_Math_Deg_To_Rad(M0); + + p2 = (double)GPS_PI * (double)2.0; + po2 = (double)GPS_PI / (double)2.0; + if(M0>GPS_PI) + M0 -= p2; + + a2 = a*a; + b2 = b*b; + e2 = (a2-b2)/a2; + e4 = e2*e2; + e6 = e2*e4; + + Ra = a*((double)1.-e2/(double)6.-(double)17.*e4/(double)360.- + (double)67.*e6/(double)3024.); + phi0c = cos(phi0); + Rac = Ra * phi0c; + + dlam = lambda - M0; + if(dlam>GPS_PI) + dlam -= p2; + if(dlam<-GPS_PI) + dlam += p2; + + *E = Rac * dlam + E0; + *N = Ra * phi + N0; + + return; +} + + + + +/* @func GPS_Math_Cyled_EN_To_LatLon ********************************** +** +** Convert cylindrical equidistant easting and northing projection +** to latitude and longitude +** +** @param [r] E [double] easting (metre) +** @param [r] N [double] northing (metre) +** @param [w] phi [double *] latitude (deg) +** @param [w] lambda [double *] longitude (deg) +** @param [r] phi0 [double] latitude of origin (deg) +** @param [r] M0 [double] central meridian (deg) +** @param [r] E0 [double] false easting +** @param [r] N0 [double] false northing +** @param [r] a [double] semi-major axis +** @param [r] b [double] semi-minor axis +** +** @return [void] +************************************************************************/ +void GPS_Math_Cyled_EN_To_LatLon(double E, double N, double *phi, + double *lambda, double phi0, double M0, + double E0, double N0, double a, double b) +{ + double p2; + double po2; + double a2; + double b2; + double e2; + double e4; + double e6; + double Ra; + double Rac; + double phi0c; + + double dx; + double dy; + + + phi0 = GPS_Math_Deg_To_Rad(phi0); + M0 = GPS_Math_Deg_To_Rad(M0); + + p2 = (double)GPS_PI * (double)2.0; + po2 = (double)GPS_PI / (double)2.0; + if(M0>GPS_PI) + M0 -= p2; + + a2 = a*a; + b2 = b*b; + e2 = (a2-b2)/a2; + e4 = e2*e2; + e6 = e2*e4; + + Ra = a*((double)1.-e2/(double)6.-(double)17.*e4/(double)360.- + (double)67.*e6/(double)3024.); + phi0c = cos(phi0); + Rac = Ra * phi0c; + + dx = E - E0; + dy = N - N0; + + if(!Rac) + *lambda = (double)0.; + else + *lambda = M0 + dx / Rac; + + *phi = dy/Ra; + + if(*phi>po2) + *phi = po2; + else if(*phi<-po2) + *phi = -po2; + + if(*lambda>GPS_PI) + *lambda -= p2; + if(*lambda<-GPS_PI) + *lambda += p2; + + if(*lambda>GPS_PI) + *lambda = GPS_PI; + else if(*lambda<-GPS_PI) + *lambda=-GPS_PI; + + *lambda = GPS_Math_Rad_To_Deg(*lambda); + *phi = GPS_Math_Rad_To_Deg(*phi); + + return; +} + + + + +/* @func GPS_Math_VderGrinten_LatLon_To_EN ********************************** +** +** Convert latitude and longitude to Van der Grinten polyconic projection +** easting and northing +** +** @param [r] phi [double] latitude (deg) +** @param [r] lambda [double] longitude (deg) +** @param [w] E [double *] easting (metre) +** @param [w] N [double *] northing (metre) +** @param [r] M0 [double] central meridian (deg) +** @param [r] E0 [double] false easting +** @param [r] N0 [double] false northing +** @param [r] a [double] semi-major axis +** @param [r] b [double] semi-minor axis +** +** @return [void] +************************************************************************/ +void GPS_Math_VderGrinten_LatLon_To_EN(double phi, double lambda, double *E, + double *N, double M0, double E0, + double N0, double a, double b) +{ + double po2; + double p2; + double a2; + double b2; + double e2; + double e4; + double e6; + double Ra; + double pRa; + + double gg; + double pp; + double pp2; + double gm0; + double ppa; + double thetai; + double theta; + double thetas; + double thetac; + double qq; + double tol; + double aa; + double aa2; + double dlam; + + lambda = GPS_Math_Deg_To_Rad(lambda); + phi = GPS_Math_Deg_To_Rad(phi); + M0 = GPS_Math_Deg_To_Rad(M0); + + p2 = (double)GPS_PI * (double)2.0; + po2 = (double)GPS_PI / (double)2.0; + if(M0>GPS_PI) + M0 -= p2; + + + a2 = a*a; + b2 = b*b; + e2 = (a2-b2)/a2; + e4 = e2*e2; + e6 = e2*e4; + + Ra = a*((double)1.-e2/(double)6.-(double)17.*e4/(double)360.-(double)67.* + e6/(double)3024.); + pRa = (double)GPS_PI * Ra; + + dlam = lambda - M0; + if(dlam>GPS_PI) + dlam -= p2; + if(dlam<-GPS_PI) + dlam += p2; + + tol = (double)1.0e-5; + + if(!phi) + { + *N = (double)0.0; + *E = Ra*dlam+E0; + } + else if(!dlam || (((po2-tol)(double)1.) + thetai=(double)1.; + else if(thetai<(double)-1.) + thetai=(double)-1.; + + theta = asin(thetai); + *E = 0; + *N = pRa * tan(theta/(double)2.) * N0; + if(phi<(double)0.0) + *N *= (double)-1.; + } + else + { + aa = (double).5*fabs((double)GPS_PI/dlam - dlam/(double)GPS_PI); + thetai = fabs(((double)2./(double)GPS_PI) * phi); + if(thetai>(double)1.) + thetai=(double)1.; + else if(thetai<(double)-1.) + thetai=(double)-1.; + + theta = asin(thetai); + thetas = sin(theta); + thetac = cos(theta); + gg = thetac/(thetas+thetac-(double)1.); + pp = gg*((double)2./thetas-(double)1.); + aa2 = aa*aa; + pp2 = pp*pp; + gm0 = gg-pp2; + ppa = pp2+aa2; + qq = aa2+gg; + *E = pRa*(aa*gm0+pow(aa2*gm0*gm0-ppa*(gg*gg-pp2),(double).5))/ppa+E0; + if(dlam<(double)0.0) + *E *= (double)-1.; + *N = pRa*(pp*qq-aa*pow((aa2+(double)1.)*ppa-qq*qq,(double).5))/ppa+N0; + if(phi<(double)0.0) + *N *= (double)-1.; + } + + return; +} + + + + +/* @func GPS_Math_VderGrinten_EN_To_LatLon ********************************** +** +** Convert Van der Grinten polyconic easting and northing projection +** to latitude and longitude +** +** @param [r] E [double] easting (metre) +** @param [r] N [double] northing (metre) +** @param [w] phi [double *] latitude (deg) +** @param [w] lambda [double *] longitude (deg) +** @param [r] M0 [double] central meridian (deg) +** @param [r] E0 [double] false easting +** @param [r] N0 [double] false northing +** @param [r] a [double] semi-major axis +** @param [r] b [double] semi-minor axis +** +** @return [void] +************************************************************************/ +void GPS_Math_VderGrinten_EN_To_LatLon(double E, double N, double *phi, + double *lambda, double M0, double E0, + double N0, double a, double b) +{ + double po2; + double p2; + double a2; + double b2; + double e2; + double e4; + double e6; + double Ra; + double pRa; + + double dx; + double dy; + double xx; + double xx2; + double yy; + double yy2; + double tyy2; + double xpy; + double c1; + double c2; + double c3; + double c3c3; + double co; + double dd; + double a1; + double m1; + double i; + double theta; + + M0 = GPS_Math_Deg_To_Rad(M0); + + p2 = (double)GPS_PI * (double)2.0; + po2 = (double)GPS_PI / (double)2.0; + if(M0>GPS_PI) + M0 -= p2; + + + a2 = a*a; + b2 = b*b; + e2 = (a2-b2)/a2; + e4 = e2*e2; + e6 = e2*e4; + + Ra = a*((double)1.-e2/(double)6.-(double)17.*e4/(double)360.-(double)67.* + e6/(double)3024.); + pRa = (double)GPS_PI * Ra; + + + dx = E - E0; + dy = N - N0; + xx = dx/pRa; + yy = dy/pRa; + xx2 = xx*xx; + yy2 = yy*yy; + xpy = xx2+yy2; + tyy2 = yy2*(double)2.; + + if(!N) + *phi=(double)0.0; + else + { + c1 = -fabs(yy)*((double)1.+xpy); + c2 = c1-tyy2+xx2; + c3 = (double)-2.*c1+(double)1.+tyy2+xpy*xpy; + co = c2/((double)3.*c3); + c3c3 = c3*c3; + dd = yy2/c3+(((double)2.*c2*c2*c2)/(c3c3*c3)-((double)9.*c1*c2)/ + c3c3)/(double)27.; + a1 = (c1-c2*co)/c3; + m1 = (double)2.* pow(-((double)1./(double)3.)*a1,(double).5); + i = (double)3.*dd/(a1*m1); + if((i>(double)1.)||(i<(double)-1.)) + *phi=po2; + else + { + theta = ((double)1./(double)3.)*acos((double)3.*dd/(a1*m1)); + *phi = (double)GPS_PI*(-m1*cos(theta+(double)GPS_PI/(double)3.)- + co); + } + } + + if(N<(double)0.0) + *phi *= (double)-1.0; + + if(!xx) + *lambda = M0; + else + *lambda = (double)GPS_PI * (xpy-(double)1.+ + pow((double)1.+((double)2.*xx2-tyy2)+xpy*xpy,(double).5)) / + ((double)2.*xx) + M0; + if(*phi>po2) + *phi = po2; + else if(*phi<-po2) + *phi = -po2; + + if(*lambda>GPS_PI) + *lambda -= p2; + if(*lambda<-GPS_PI) + *lambda += p2; + + if(*lambda>GPS_PI) + *lambda = GPS_PI; + else if(*lambda<-GPS_PI) + *lambda=-GPS_PI; + + *lambda = GPS_Math_Rad_To_Deg(*lambda); + *phi = GPS_Math_Rad_To_Deg(*phi); + + return; +} + + + + +/* @func GPS_Math_Bonne_LatLon_To_EN ********************************** +** +** Convert latitude and longitude to Bonne pseudoconic equal area projection +** easting and northing +** +** @param [r] phi [double] latitude (deg) +** @param [r] lambda [double] longitude (deg) +** @param [w] E [double *] easting (metre) +** @param [w] N [double *] northing (metre) +** @param [r] phi1 [double] latitude of true scale (deg) +** @param [r] lambda1 [double] longitude from pole (deg) +** @param [r] E0 [double] false easting +** @param [r] N0 [double] false northing +** @param [r] a [double] semi-major axis +** @param [r] b [double] semi-minor axis +** +** @return [void] +************************************************************************/ +void GPS_Math_PolarSt_LatLon_To_EN(double phi, double lambda, double *E, + double *N, double phi1, double lambda1, + double E0, double N0, double a, double b) +{ + double p2; + double po2; + double a2; + double b2; + double e2; + double e4=(double)0.; + double e; + double eo2; + double sh; + double mc; + double tc=(double)0.; + double amc=(double)0.; + double ta; + double phi1s; + double phi1c; + double es; + double op; + double om; + double pe; + double polat; + double polon; + + double dlam; + double phis; + double t; + double rho; + + + lambda1 = GPS_Math_Deg_To_Rad(lambda1); + phi1 = GPS_Math_Deg_To_Rad(phi1); + lambda = GPS_Math_Deg_To_Rad(lambda); + phi = GPS_Math_Deg_To_Rad(phi); + + + p2 = (double)GPS_PI * (double)2.0; + po2 = (double)GPS_PI / (double)2.0; + + ta = a * (double)2.0; + if(lambda1>GPS_PI) + lambda1 -= p2; + if(phi1<(double)0.0) + { + sh=(double)1.0; + polat = -phi1; + polon = -lambda1; + } + else + { + sh=(double)0.0; + polat = phi1; + polon = lambda1; + } + + a2 = a*a; + b2 = b*b; + e2 = (a2-b2)/a2; + e = pow(e2,(double).5); + eo2 = e/(double)2.; + + if(fabs(fabs(polat)-po2)>(double)1.0e-10) + { + phi1s = sin(polat); + phi1c = cos(polat); + es = e*phi1s; + pe = pow(((double)1.-es)/((double)1.+es),eo2); + mc = phi1c / pow((double)1.-es*es,(double).5); + amc = mc * a; + tc = tan(((double)GPS_PI/(double)4.)-polat/(double)2.) / pe; + } + else + { + op = (double)1. + e; + om = (double)1. - e; + e4 = pow(pow(op,op)*pow(om,om),(double).5); + } + + + + if(fabs(fabs(phi)-po2)<(double)1.0e-10) + *E = *N = (double)0.0; + else + { + if(sh) + { + phi *= (double)-1.0; + lambda *= (double)-1.0; + } + + dlam = lambda - polon; + if(dlam>GPS_PI) + dlam -= p2; + if(dlam<-GPS_PI) + dlam += p2; + phis = sin(phi); + es = e * phis; + pe = pow(((double)1.-es)/((double)1.+es),eo2); + t = tan(((double)GPS_PI/(double)4.)-phi/(double)2.) / pe; + + if(fabs(fabs(polat)-po2)>(double)1.0e-10) + rho = amc * t / tc; + else + rho = ta * t / e4; + *E = rho * sin(dlam) + E0; + + if(sh) + { + *E *= (double)-1.; + *N = rho * cos(dlam) + N0; + } + else + *N = -rho * cos(dlam) + N0; + } + + return; +} + + + + +/* @func GPS_Math_PolarSt_EN_To_LatLon ********************************** +** +** Convert Polar Stereographic easting and northing projection +** to latitude and longitude +** +** @param [r] E [double] easting (metre) +** @param [r] N [double] northing (metre) +** @param [w] phi [double *] latitude (deg) +** @param [w] lambda [double *] longitude (deg) +** @param [r] phi1 [double] latitude of true scale (deg) +** @param [r] lambda1 [double] longitude from pole (deg) +** @param [r] E0 [double] false easting +** @param [r] N0 [double] false northing +** @param [r] a [double] semi-major axis +** @param [r] b [double] semi-minor axis +** +** @return [void] +************************************************************************/ +void GPS_Math_PolarSt_EN_To_LatLon(double E, double N, double *phi, + double *lambda, double phi1, double lambda1, + double E0, double N0, double a, double b) +{ + double p2; + double po2; + double a2; + double b2; + double e2; + double e4=(double)0.; + double e; + double eo2; + double sh; + double mc; + double tc=(double)0.; + double amc=(double)0.; + double ta; + double phi1s; + double phi1c; + double es; + double op; + double om; + double pe; + double polat; + double polon; + + double dx; + double dy; + double t; + double rho; + double PHI; + double PHIS; + double TPHI; + + + lambda1 = GPS_Math_Deg_To_Rad(lambda1); + phi1 = GPS_Math_Deg_To_Rad(phi1); + + + p2 = (double)GPS_PI * (double)2.0; + po2 = (double)GPS_PI / (double)2.0; + + ta = a * (double)2.0; + if(lambda1>GPS_PI) + lambda1 -= p2; + if(phi1<(double)0.0) + { + sh=(double)1.0; + polat = -phi1; + polon = -lambda1; + } + else + { + sh=(double)0.0; + polat = phi1; + polon = lambda1; + } + + a2 = a*a; + b2 = b*b; + e2 = (a2-b2)/a2; + e = pow(e2,(double).5); + eo2 = e/(double)2.; + + if(fabs(fabs(polat)-po2)>(double)1.0e-10) + { + phi1s = sin(polat); + phi1c = cos(polat); + es = e*phi1s; + pe = pow(((double)1.-es)/((double)1.+es),eo2); + mc = phi1c / pow((double)1.-es*es,(double).5); + amc = mc * a; + tc = tan(((double)GPS_PI/(double)4.)-polat/(double)2.) / pe; + } + else + { + op = (double)1. + e; + om = (double)1. - e; + e4 = pow(pow(op,op)*pow(om,om),(double).5); + } + + + dx = E - E0; + dy = N - N0; + if(!dx && !dy) + { + *phi = po2; + *lambda = polon; + } + else + { + if(sh) + { + dx *= (double)-1.; + dy *= (double)-1.; + } + rho = pow(dx*dx+dy*dy,(double).5); + if(fabs(fabs(polat)-po2)>(double)1.0e-10) + t = rho * tc / amc; + else + t = rho * e4 / ta; + TPHI = (double)0.0; + PHI = po2 - (double)2.*atan(t); + while(fabs(PHI-TPHI)>(double)1.0e-10) + { + TPHI=PHI; + PHIS = sin(PHI); + es = e * PHIS; + pe = pow(((double)1.-es)/((double)1.+es),eo2); + PHI = po2 - (double)2. * atan(t*pe); + } + *phi = PHI; + *lambda = polon + atan2(dx,-dy); + + if(*phi>po2) + *phi = po2; + else if(*phi<-po2) + *phi = -po2; + + if(*lambda>GPS_PI) + *lambda -= p2; + if(*lambda<-GPS_PI) + *lambda += p2; + + if(*lambda>GPS_PI) + *lambda = GPS_PI; + else if(*lambda<-GPS_PI) + *lambda=-GPS_PI; + } + if(sh) + { + *phi *= (double)-1.; + *lambda *= (double)1.; + } + + *lambda = GPS_Math_Rad_To_Deg(*lambda); + *phi = GPS_Math_Rad_To_Deg(*phi); + + return; +} + + + + +/* @func GPS_Math_Mollweide_LatLon_To_EN ********************************** +** +** Convert latitude and longitude to Mollweide projection easting and +** northing +** +** @param [r] phi [double] latitude (deg) +** @param [r] lambda [double] longitude (deg) +** @param [w] E [double *] easting (metre) +** @param [w] N [double *] northing (metre) +** @param [r] M0 [double] central meridian (deg) +** @param [r] E0 [double] false easting +** @param [r] N0 [double] false northing +** @param [r] a [double] semi-major axis +** @param [r] b [double] semi-minor axis +** +** @return [void] +************************************************************************/ +void GPS_Math_Mollweide_LatLon_To_EN(double phi, double lambda, double *E, + double *N, double M0, double E0, + double N0, double a, double b) +{ + double a2; + double b2; + double e2; + double e4; + double e6; + double p2; + double po2; + double Ra; + double sRa2; + double sRa8; + + double ps; + double dlam; + double theta; + double thetap; + double d; + double tol; + + phi = GPS_Math_Deg_To_Rad(phi); + lambda = GPS_Math_Deg_To_Rad(lambda); + M0 = GPS_Math_Deg_To_Rad(M0); + + po2 = (double)GPS_PI / (double)2.0; + p2 = (double)GPS_PI * (double)2.0; + a2 = a*a; + b2 = b*b; + e2 = (a2-b2)/a2; + e4 = e2*e2; + e6 = e4*e2; + + Ra = a*((double)1.0-e2/(double)6.0-(double)17.0*e4/(double)360.0- + (double)67.0*e6/(double)3024.0); + sRa2 = pow((double)2.,(double).5) * Ra; + sRa8 = pow((double)8.,(double).5) * Ra; + + if(M0>GPS_PI) + M0 -= p2; + + ps = sin(phi) * (double)GPS_PI; + d = (double)0.1745329; + tol = (double)4.85e-10; + thetap = phi; + + dlam = lambda - M0; + if(dlam>GPS_PI) + dlam-=p2; + if(dlam<-GPS_PI) + dlam+=p2; + + while(fabs(d)>tol) + { + d = -(thetap+sin(thetap)-ps)/((double)1.+cos(thetap)); + thetap += d; + } + theta = thetap / (double)2.; + *E = (sRa8/(double)GPS_PI) * dlam * cos(theta) + E0; + *N = sRa2 * sin(theta) + N0; + + return; +} + + + + +/* @func GPS_Math_Mollweide_EN_To_LatLon ********************************** +** +** Convert latitude and longitude to Mollweide projection easting and +** northing +** +** @param [r] E [double] easting (metre) +** @param [r] N [double] northing (metre) +** @param [w] phi [double *] latitude (deg) +** @param [w] lambda [double *] longitude (deg) +** @param [r] M0 [double] central meridian (deg) +** @param [r] E0 [double] false easting +** @param [r] N0 [double] false northing +** @param [r] a [double] semi-major axis +** @param [r] b [double] semi-minor axis +** +** @return [void] +************************************************************************/ +void GPS_Math_Mollweide_EN_To_LatLon(double E, double N, double *phi, + double *lambda, double M0, double E0, + double N0, double a, double b) +{ + double a2; + double b2; + double e2; + double e4; + double e6; + double p2; + double po2; + double Ra; + double sRa2; + double sRa8; + + double dx; + double dy; + double theta=(double)0.; + double tt; + double i; + + M0 = GPS_Math_Deg_To_Rad(M0); + + po2 = (double)GPS_PI / (double)2.0; + p2 = (double)GPS_PI * (double)2.0; + a2 = a*a; + b2 = b*b; + e2 = (a2-b2)/a2; + e4 = e2*e2; + e6 = e4*e2; + + Ra = a*((double)1.0-e2/(double)6.0-(double)17.0*e4/(double)360.0- + (double)67.0*e6/(double)3024.0); + sRa2 = pow((double)2.,(double).5) * Ra; + sRa8 = pow((double)8.,(double).5) * Ra; + + if(M0>GPS_PI) + M0 -= p2; + + dx = E - E0; + dy = N - N0; + i = dy/sRa2; + if(fabs(i)>(double)1.) + { + *phi = po2; + if(N<(double)0.0) + *phi *= (double)-1.; + } + else + { + theta = asin(i); + tt = theta * (double)2.; + *phi = asin((tt+sin(tt))/(double)GPS_PI); + if(*phi>po2) + *phi=po2; + else if (*phi<-po2) + *phi=-po2; + } + + if(fabs(fabs(*phi)-po2)<(double)1.0e-10) + *lambda = M0; + else + *lambda = M0 + (double)GPS_PI * dx / (sRa8 * cos(theta)); + + + if(*lambda>GPS_PI) + *lambda-=p2; + if(*lambda<-GPS_PI) + *lambda+=p2; + + if(*lambda>GPS_PI) + *lambda=GPS_PI; + else if(*lambda<-GPS_PI) + *lambda=-GPS_PI; + + *lambda = GPS_Math_Rad_To_Deg(*lambda); + *phi = GPS_Math_Rad_To_Deg(*phi); + + return; +} + + + + +/* @func GPS_Math_Orthog_LatLon_To_EN ********************************** +** +** Convert latitude and longitude to orthographic projection +** easting and northing +** +** @param [r] phi [double] latitude (deg) +** @param [r] lambda [double] longitude (deg) +** @param [w] E [double *] easting (metre) +** @param [w] N [double *] northing (metre) +** @param [r] phi0 [double] latitude of origin (deg) +** @param [r] lambda0 [double] longitude of origin (deg) +** @param [r] E0 [double] false easting +** @param [r] N0 [double] false northing +** @param [r] a [double] semi-major axis +** @param [r] b [double] semi-minor axis +** +** @return [void] +************************************************************************/ +void GPS_Math_Orthog_LatLon_To_EN(double phi, double lambda, double *E, + double *N, double phi0, double lambda0, + double E0, double N0, double a, double b) +{ + double p2; + double po2; + double a2; + double b2; + double e2; + double e4; + double e6; + double Ra; + double phi0s; + double phi0c; + + double phis; + double phic; + double dlam; + double clc; + double cc; + + + lambda = GPS_Math_Deg_To_Rad(lambda); + phi = GPS_Math_Deg_To_Rad(phi); + phi0 = GPS_Math_Deg_To_Rad(phi0); + lambda0 = GPS_Math_Deg_To_Rad(lambda0); + + p2 = (double)GPS_PI * (double)2.0; + po2 = (double)GPS_PI / (double)2.0; + if(lambda0>GPS_PI) + lambda0 -= p2; + a2 = a*a; + b2 = b*b; + e2 = (a2-b2)/a2; + e4 = e2*e2; + e6 = e2*e4; + Ra = a * ((double)1.-e2/(double)6.-(double)17.*e4/(double)360.-(double)67.* + e6/(double)3024.); + phi0s = sin(phi0); + phi0c = cos(phi0); + + dlam = lambda - lambda0; + if(dlam>GPS_PI) + dlam -= p2; + if(dlam<-GPS_PI) + dlam += p2; + + + phis = sin(phi); + phic = cos(phi); + clc = phic * cos(dlam); + cc = phi0s * phis + phi0c * clc; + + *E = Ra * phic * sin(dlam) + E0; + *N = Ra * (phi0c * phis - phi0s * clc) + N0; + + return; +} + + + + +/* @func GPS_Math_Orthog_EN_To_LatLon ********************************** +** +** Convert Orthogonal easting and northing projection +** to latitude and longitude +** +** @param [r] E [double] easting (metre) +** @param [r] N [double] northing (metre) +** @param [w] phi [double *] latitude (deg) +** @param [w] lambda [double *] longitude (deg) +** @param [r] phi0 [double] latitude of origin (deg) +** @param [r] lambda0 [double] longitude of origin (deg) +** @param [r] E0 [double] false easting +** @param [r] N0 [double] false northing +** @param [r] a [double] semi-major axis +** @param [r] b [double] semi-minor axis +** +** @return [void] +************************************************************************/ +void GPS_Math_Orthog_EN_To_LatLon(double E, double N, double *phi, + double *lambda, double phi0, double lambda0, + double E0, double N0, double a, double b) +{ + double p2; + double po2; + double a2; + double b2; + double e2; + double e4; + double e6; + double Ra; + double phi0s; + double phi0c; + + double dx; + double dy; + double rho; + double adod; + double ror; + double cc; + double ccs; + double ccc; + + + + + phi0 = GPS_Math_Deg_To_Rad(phi0); + lambda0 = GPS_Math_Deg_To_Rad(lambda0); + + p2 = (double)GPS_PI * (double)2.0; + po2 = (double)GPS_PI / (double)2.0; + if(lambda0>GPS_PI) + lambda0 -= p2; + a2 = a*a; + b2 = b*b; + e2 = (a2-b2)/a2; + e4 = e2*e2; + e6 = e2*e4; + Ra = a * ((double)1.-e2/(double)6.-(double)17.*e4/(double)360.-(double)67.* + e6/(double)3024.); + phi0s = sin(phi0); + phi0c = cos(phi0); + + + dx = E - E0; + dy = N - N0; + adod = atan(dx/dy); + rho = pow(dx*dx+dy*dy,(double).5); + if(!rho) + { + *phi = phi0; + *lambda = lambda0; + } + else + { + ror = rho/Ra; + if(ror>(double)1.) + ror=(double)1.; + else if(ror<(double)-1.) + ror=(double)-1.; + cc = asin(ror); + ccs = sin(cc); + ccc = cos(cc); + *phi = asin(ccc*phi0s+(dy*ccs*phi0c/rho)); + if(phi0==po2) + *lambda = lambda0 - adod; + else if(phi0==-po2) + *lambda = lambda0 + adod; + else + *lambda = lambda0+atan(dx*ccs/(rho*phi0c*ccc-dy*phi0s*ccs)); + } + + if(*phi>po2) + *phi = po2; + else if(*phi<-po2) + *phi = -po2; + + if(*lambda>GPS_PI) + *lambda -= p2; + if(*lambda<-GPS_PI) + *lambda += p2; + + if(*lambda>GPS_PI) + *lambda = GPS_PI; + else if(*lambda<-GPS_PI) + *lambda=-GPS_PI; + + *lambda = GPS_Math_Rad_To_Deg(*lambda); + *phi = GPS_Math_Rad_To_Deg(*phi); + + return; +} + + + + +/* @func GPS_Math_Polycon_LatLon_To_EN ********************************** +** +** Convert latitude and longitude to Polyconic projection +** easting and northing +** +** @param [r] phi [double] latitude (deg) +** @param [r] lambda [double] longitude (deg) +** @param [w] E [double *] easting (metre) +** @param [w] N [double *] northing (metre) +** @param [r] phi0 [double] latitude of origin (deg) +** @param [r] M0 [double] central meridian (deg) +** @param [r] E0 [double] false easting +** @param [r] N0 [double] false northing +** @param [r] a [double] semi-major axis +** @param [r] b [double] semi-minor axis +** +** @return [void] +************************************************************************/ +void GPS_Math_Polycon_LatLon_To_EN(double phi, double lambda, double *E, + double *N, double phi0, double M0, + double E0, double N0, double a, double b) +{ + double p2; + double po2; + double a2; + double b2; + double e2; + double e4; + double e6; + double AM0; + double c0; + double c1; + double c2; + double c3; + double j; + double te4; + double phi0s2; + double phi0s4; + double phi0s6; + + double phis; + double phis2; + double phis4; + double phis6; + double dlam; + double NN; + double NNot; + double MM; + double EE; + double lat; + + + lambda = GPS_Math_Deg_To_Rad(lambda); + phi = GPS_Math_Deg_To_Rad(phi); + phi0 = GPS_Math_Deg_To_Rad(phi0); + M0 = GPS_Math_Deg_To_Rad(M0); + + p2 = (double)GPS_PI * (double)2.0; + po2 = (double)GPS_PI / (double)2.0; + if(M0>GPS_PI) + M0 -= p2; + a2 = a*a; + b2 = b*b; + e2 = (a2-b2)/a2; + e4 = e2*e2; + e6 = e2*e4; + + j = (double)45.0*e6/(double)1024.0; + te4 = (double)3.0 * e4; + c0 = (double)1.0-e2/(double)4.0-te4/(double)64.0-(double)5.0*e6/ + (double)256.0; + c1 = (double)3.0*e2/(double)8.0+te4/(double)32.0+j; + c2 = (double)15.0*e4/(double)256.0+j; + c3 = (double)35.0*e6/(double)3072.0; + + lat = c0 * phi0; + + phi0s2 = c1 * sin((double)2.0*phi0); + phi0s4 = c2 * sin((double)4.0*phi0); + phi0s6 = c3 * sin((double)6.0*phi0); + AM0 = a*(lat-phi0s2+phi0s4-phi0s6); + + + + dlam = lambda - M0; + if(dlam>GPS_PI) + dlam -= p2; + if(dlam<-GPS_PI) + dlam += p2; + + phis = sin(phi); + + if(!phi) + { + *E = a * dlam + E0; + *N = -AM0 + N0; + } + else + { + NN = a / pow((double)1.-e2*phis*phis,(double).5); + NNot = NN / tan(phi); + lat = c0 * phi; + phis2 = c1 * sin((double)2.0*phi); + phis4 = c2 * sin((double)4.0*phi); + phis6 = c3 * sin((double)6.0*phi); + MM = a*(lat-phis2+phis4-phis6); + EE = dlam *phis; + *E = NNot * sin(EE) + E0; + *N = MM - AM0 + NNot * ((double)1.-cos(EE)) + N0; + } + + return; +} + + + + +/* @func GPS_Math_Polycon_EN_To_LatLon ********************************** +** +** Convert Polyconic easting and northing projection +** to latitude and longitude +** +** @param [r] E [double] easting (metre) +** @param [r] N [double] northing (metre) +** @param [w] phi [double *] latitude (deg) +** @param [w] lambda [double *] longitude (deg) +** @param [r] phi0 [double] latitude of origin (deg) +** @param [r] M0 [double] central meridian (deg) +** @param [r] E0 [double] false easting +** @param [r] N0 [double] false northing +** @param [r] a [double] semi-major axis +** @param [r] b [double] semi-minor axis +** +** @return [void] +************************************************************************/ +void GPS_Math_Polycon_EN_To_LatLon(double E, double N, double *phi, + double *lambda, double phi0, double M0, + double E0, double N0, double a, double b) +{ + double p2; + double po2; + double a2; + double b2; + double e2; + double e4; + double e6; + double AM0; + double c0; + double c1; + double c2; + double c3; + double j; + double te4; + double phi0s2; + double phi0s4; + double phi0s6; + + double dx; + double dy; + double dxoa; + double AA; + double BB; + double CC=(double)0.; + double PHIn; + double PHId; + double PHIs; + double PHI; + double PHIs2; + double PHIs4; + double PHIs6; + double Mn; + double Mnp; + double Ma; + double AAMa; + double mpb; + double AAmin; + double tol; + double lat; + + + phi0 = GPS_Math_Deg_To_Rad(phi0); + M0 = GPS_Math_Deg_To_Rad(M0); + + p2 = (double)GPS_PI * (double)2.0; + po2 = (double)GPS_PI / (double)2.0; + if(M0>GPS_PI) + M0 -= p2; + a2 = a*a; + b2 = b*b; + e2 = (a2-b2)/a2; + e4 = e2*e2; + e6 = e2*e4; + + j = (double)45.0*e6/(double)1024.0; + te4 = (double)3.0 * e4; + c0 = (double)1.0-e2/(double)4.0-te4/(double)64.0-(double)5.0*e6/ + (double)256.0; + c1 = (double)3.0*e2/(double)8.0+te4/(double)32.0+j; + c2 = (double)15.0*e4/(double)256.0+j; + c3 = (double)35.0*e6/(double)3072.0; + + lat = c0 * phi0; + + phi0s2 = c1 * sin((double)2.0*phi0); + phi0s4 = c2 * sin((double)4.0*phi0); + phi0s6 = c3 * sin((double)6.0*phi0); + AM0 = a*(lat-phi0s2+phi0s4-phi0s6); + + tol = (double)1.0e-12; + + dx = E - E0; + dy = N - N0; + dxoa = dx/a; + if((((-AM0-(double)1.)tol) + { + PHIs = sin(PHIn); + CC = pow((double)1.-e2*PHIs*PHIs,(double).5) * tan(PHIn); + PHI = PHIn * c0; + PHIs2 = c1 * sin((double)2.0*PHIn); + PHIs4 = c2 * sin((double)4.0*PHIn); + PHIs6 = c3 * sin((double)6.0*PHIn); + Mn = a*(PHI-PHIs2+PHIs4-PHIs6); + Mnp = c0 - (double)2.*c1*cos((double)2.*PHIn)+(double)4.*c2* + cos((double)4.*PHIn)-(double)6.*c3*cos((double)6.*PHIn); + Ma = Mn / a; + AAMa = AA * Ma; + mpb = Ma*Ma+BB; + AAmin = AA - Ma; + PHId = (AAMa*CC+AAmin-(double).5*mpb*CC)/ + (e2*PHIs2*(mpb-(double)2.*AAMa) / + (double)4.*CC+AAmin*(CC*Mnp-(double)2./PHIs2)-Mnp); + PHIn -= PHId; + } + *phi = PHIn; + + if(*phi>po2) + *phi = po2; + else if(*phi<-po2) + *phi = -po2; + + if((((po2-(double).00001)GPS_PI) + *lambda -= p2; + if(*lambda<-GPS_PI) + *lambda += p2; + + if(*lambda>GPS_PI) + *lambda = GPS_PI; + else if(*lambda<-GPS_PI) + *lambda=-GPS_PI; + + *lambda = GPS_Math_Rad_To_Deg(*lambda); + *phi = GPS_Math_Rad_To_Deg(*phi); + + return; +} + + + + +/* @func GPS_Math_Sinusoid_LatLon_To_EN ********************************** +** +** Convert latitude and longitude to Sinusoidal projection easting and +** northing +** +** @param [r] phi [double] latitude (deg) +** @param [r] lambda [double] longitude (deg) +** @param [w] E [double *] easting (metre) +** @param [w] N [double *] northing (metre) +** @param [r] M0 [double] central meridian (deg) +** @param [r] E0 [double] false easting +** @param [r] N0 [double] false northing +** @param [r] a [double] semi-major axis +** @param [r] b [double] semi-minor axis +** +** @return [void] +************************************************************************/ +void GPS_Math_Sinusoid_LatLon_To_EN(double phi, double lambda, double *E, + double *N, double M0, double E0, + double N0, double a, double b) +{ + double a2; + double b2; + double e2; + double e4; + double e6; + double p2; + double po2; + double c0; + double c1; + double c2; + double c3; + double A1; + double A0; + double A2; + double A3; + double E1; + double E2; + double E3; + double E4; + double j; + double om0; + double som0; + + double phis; + double phis2; + double phis4; + double phis6; + double mm; + double MM; + double dlam; + + + phi = GPS_Math_Deg_To_Rad(phi); + lambda = GPS_Math_Deg_To_Rad(lambda); + M0 = GPS_Math_Deg_To_Rad(M0); + + po2 = (double)GPS_PI / (double)2.0; + p2 = (double)GPS_PI * (double)2.0; + a2 = a*a; + b2 = b*b; + e2 = (a2-b2)/a2; + e4 = e2*e2; + e6 = e4*e2; + + j = (double)45.*e6/(double)1024.; + c0 = (double)1.-e2/(double)4.-(double)3.*e4/(double)64.-(double)5.* + e6/(double)256.; + c1 = (double)3.*e2/(double)8.+(double)3.*e4/(double)32.+j; + c2 = (double)15.*e4/(double)256.+j; + c3 = (double)35.*e6/(double)3072.; + om0 = (double)1. - e2; + som0 = pow(om0,(double).5); + E1 = ((double)1.-som0)/((double)1.+som0); + E2 = E1*E1; + E3 = E1*E2; + E4 = E1*E3; + A0 = (double)3.*E1/(double)2.-(double)27.*E3/(double)32.; + A1 = (double)21.*E2/(double)16.-(double)55.*E4/(double)32.; + A2 = (double)151.*E3/(double)96.; + A3 = (double)1097.*E4/(double)512.; + + if(M0>GPS_PI) + M0 -= p2; + + phis = sin(phi); + + dlam = lambda - M0; + if(dlam>GPS_PI) + dlam-=p2; + if(dlam<-GPS_PI) + dlam+=p2; + + mm = pow((double)1.-e2*phis*phis,(double).5); + phis2 = c1 * sin((double)2.*phi); + phis4 = c2 * sin((double)4.*phi); + phis6 = c3 * sin((double)6.*phi); + MM = a * (c0*phi-phis2+phis4-phis6); + + + + *E = a*dlam*cos(phi)/mm+E0; + *N = MM + N0; + + return; +} + + + + +/* @func GPS_Math_Sinusoid_EN_To_LatLon ********************************** +** +** Convert latitude and longitude to Sinusoidal projection easting and +** northing +** +** @param [r] E [double] easting (metre) +** @param [r] N [double] northing (metre) +** @param [w] phi [double *] latitude (deg) +** @param [w] lambda [double *] longitude (deg) +** @param [r] M0 [double] central meridian (deg) +** @param [r] E0 [double] false easting +** @param [r] N0 [double] false northing +** @param [r] a [double] semi-major axis +** @param [r] b [double] semi-minor axis +** +** @return [void] +************************************************************************/ +void GPS_Math_Sinusoid_EN_To_LatLon(double E, double N, double *phi, + double *lambda, double M0, double E0, + double N0, double a, double b) +{ + double a2; + double b2; + double e2; + double e4; + double e6; + double p2; + double po2; + double c0; + double c1; + double c2; + double c3; + double A1; + double A0; + double A2; + double A3; + double E1; + double E2; + double E3; + double E4; + double j; + double om0; + double som0; + + double dx; + double dy; + double mu; + double mu2s; + double mu4s; + double mu6s; + double mu8s; + double phis; + + + M0 = GPS_Math_Deg_To_Rad(M0); + + po2 = (double)GPS_PI / (double)2.0; + p2 = (double)GPS_PI * (double)2.0; + a2 = a*a; + b2 = b*b; + e2 = (a2-b2)/a2; + e4 = e2*e2; + e6 = e4*e2; + + j = (double)45.*e6/(double)1024.; + c0 = (double)1.-e2/(double)4.-(double)3.*e4/(double)64.-(double)5.* + e6/(double)256.; + c1 = (double)3.*e2/(double)8.+(double)3.*e4/(double)32.+j; + c2 = (double)15.*e4/(double)256.+j; + c3 = (double)35.*e6/(double)3072.; + om0 = (double)1. - e2; + som0 = pow(om0,(double).5); + E1 = ((double)1.-som0)/((double)1.+som0); + E2 = E1*E1; + E3 = E1*E2; + E4 = E1*E3; + A0 = (double)3.*E1/(double)2.-(double)27.*E3/(double)32.; + A1 = (double)21.*E2/(double)16.-(double)55.*E4/(double)32.; + A2 = (double)151.*E3/(double)96.; + A3 = (double)1097.*E4/(double)512.; + + + dx = E - E0; + dy = N - N0; + + mu = dy/(c0*a); + mu2s = A0 * sin((double)2.*mu); + mu4s = A1 * sin((double)4.*mu); + mu6s = A2 * sin((double)6.*mu); + mu8s = A3 * sin((double)8.*mu); + *phi = mu + mu2s + mu4s + mu6s + mu8s; + + if(*phi>po2) + *phi=po2; + else if (*phi<-po2) + *phi=-po2; + + if((((po2-(double)1.0e-8)GPS_PI) + *lambda-=p2; + if(*lambda<-GPS_PI) + *lambda+=p2; + + if(*lambda>GPS_PI) + *lambda=GPS_PI; + else if(*lambda<-GPS_PI) + *lambda=-GPS_PI; + + *lambda = GPS_Math_Rad_To_Deg(*lambda); + *phi = GPS_Math_Rad_To_Deg(*phi); + + return; +} + + + + +/* @func GPS_Math_TCylEA_LatLon_To_EN ********************************** +** +** Convert latitude and longitude to transverse cylindrical equal area +** projection easting and northing +** +** @param [r] phi [double] latitude (deg) +** @param [r] lambda [double] longitude (deg) +** @param [w] E [double *] easting (metre) +** @param [w] N [double *] northing (metre) +** @param [r] phi0 [double] latitude of origin (deg) +** @param [r] M0 [double] central meridian (deg) +** @param [r] E0 [double] false easting +** @param [r] N0 [double] false northing +** @param [r] a [double] semi-major axis +** @param [r] b [double] semi-minor axis +** +** @return [void] +************************************************************************/ +void GPS_Math_TCylEA_LatLon_To_EN(double phi, double lambda, double *E, + double *N, double phi0, double M0, double E0, + double N0, double a, double b) +{ + double p2; + double po2; + double a2; + double b2; + double e; + double e2; + double e4; + double e6; + double AM0; + double qp; + double om; + double oo; + double c0; + double c1; + double c2; + double c3; + double b0; + double b1; + double B2; + double b3; + double A0; + double A1; + double A2; + double sf; + double x; + double som; + double phis; + double j; + double te4; + double lat; + double phi0s2; + double phi0s4; + double phi0s6; + double E1; + double E2; + double E3; + double E4; + + double dlam; + double qq; + double qqo; + double bt; + double btc; + double PHI; + double PHIs2; + double PHIs4; + double PHIs6; + double bts2; + double bts4; + double bts6; + double PHIc; + double PHIcs; + double Mc; + + + + sf = (double)1.0; /* scale factor */ + + lambda = GPS_Math_Deg_To_Rad(lambda); + phi = GPS_Math_Deg_To_Rad(phi); + phi0 = GPS_Math_Deg_To_Rad(phi0); + M0 = GPS_Math_Deg_To_Rad(M0); + + p2 = (double)GPS_PI * (double)2.0; + po2 = (double)GPS_PI / (double)2.0; + if(M0>GPS_PI) + M0 -= p2; + a2 = a*a; + b2 = b*b; + e2 = (a2-b2)/a2; + e4 = e2*e2; + e6 = e2*e4; + e = pow(e2,(double).5); + om = (double)1.-e2; + som = pow(om,(double).5); + oo = (double)1./((double)2.*e); + + phis = sin(po2); + x = e * phis; + qp = om*(phis/((double)1.-e2*phis*phis)-oo* + log(((double)1.-x)/((double)1.+x))); + + A0 = e2 / (double)3.+(double)31.*e4/(double)180.+(double)517.* + e6/(double)5040.; + A1 = (double)23.*e4/(double)360.+(double)251.*e6/(double)3780.; + A2 = (double)761.*e6/(double)45360.; + + E1 = ((double)1.0-som) / ((double)1.0+som); + E2 = E1*E1; + E3 = E2*E1; + E4 = E3*E1; + + b0 = (double)3.*E1/(double)2.-(double)27.*E3/(double)32.; + b1 = (double)21.*E2/(double)16.-(double)55.*E4/(double)32.; + B2 = (double)151.*E3/(double)96.; + b3 = (double)1097.*E4/(double)512.; + + + j = (double)45.0*e6/(double)1024.0; + te4 = (double)3.0 * e4; + c0 = (double)1.0-e2/(double)4.0-te4/(double)64.0-(double)5.0*e6/ + (double)256.0; + c1 = (double)3.0*e2/(double)8.0+te4/(double)32.0+j; + c2 = (double)15.0*e4/(double)256.0+j; + c3 = (double)35.0*e6/(double)3072.0; + + lat = c0 * phi0; + + phi0s2 = c1 * sin((double)2.0*phi0); + phi0s4 = c2 * sin((double)4.0*phi0); + phi0s6 = c3 * sin((double)6.0*phi0); + AM0 = a*(lat-phi0s2+phi0s4-phi0s6); + + + dlam = lambda - M0; + if(dlam>GPS_PI) + dlam -= p2; + if(dlam<-GPS_PI) + dlam += p2; + + phis = sin(phi); + + if(phi==po2) + { + qq = qp; + qqo = (double)1.; + } + else + { + x = e * phis; + qq = om*(phis/((double)1.-e2*phis*phis)-oo* + log(((double)1.-x)/((double)1.+x))); + qqo = qq/qp; + } + + if(qqo>(double)1.) + qqo = (double)1.; + else if(qqo<(double)-1.) + qqo = (double)-1.; + + bt = asin(qqo); + btc = atan(tan(bt)/cos(dlam)); + + if((fabs(btc)-po2)>(double)1.0e-8) + PHIc = btc; + else + { + bts2 = A0 * sin((double)2.0*btc); + bts4 = A1 * sin((double)4.0*btc); + bts6 = A2 * sin((double)6.0*btc); + PHIc = btc + bts2 + bts4 + bts6; + } + + PHIcs = sin(PHIc); + *E = a*cos(bt)*cos(PHIc)*sin(dlam)/(sf*cos(btc)* + pow((double)1.-e2*PHIcs*PHIcs, + (double).5)) + E0; + PHI = c0 * PHIc; + PHIs2 = c1 * sin((double)2.0*PHIc); + PHIs4 = c2 * sin((double)4.0*PHIc); + PHIs6 = c3 * sin((double)6.0*PHIc); + Mc = a*(PHI-PHIs2+PHIs4-PHIs6); + + *N = sf * (Mc-AM0) + N0; + + return; +} + + + + +/* @func GPS_Math_TCylEA_EN_To_LatLon ********************************** +** +** Convert transverse cylindrical equal area easting and northing projection +** to latitude and longitude +** +** @param [r] E [double] easting (metre) +** @param [r] N [double] northing (metre) +** @param [w] phi [double *] latitude (deg) +** @param [w] lambda [double *] longitude (deg) +** @param [r] phi0 [double] latitude of origin (deg) +** @param [r] M0 [double] central meridian (deg) +** @param [r] E0 [double] false easting +** @param [r] N0 [double] false northing +** @param [r] a [double] semi-major axis +** @param [r] b [double] semi-minor axis +** +** @return [void] +************************************************************************/ +void GPS_Math_TCylEA_EN_To_LatLon(double E, double N, double *phi, + double *lambda, double phi0, double M0, + double E0, double N0, double a, double b) +{ + double p2; + double po2; + double a2; + double b2; + double e; + double e2; + double e4; + double e6; + double AM0; + double qp; + double om; + double oo; + double c0; + double c1; + double c2; + double c3; + double b0; + double b1; + double B2; + double b3; + double A0; + double A1; + double A2; + double sf; + double x; + double som; + double phis; + double j; + double te4; + double lat; + double phi0s2; + double phi0s4; + double phi0s6; + double E1; + double E2; + double E3; + double E4; + + double dx; + double dy; + double bt; + double btc; + double btp; + double btcc; + double Mc; + double Muc; + double mus2; + double mus4; + double mus6; + double mus8; + double bts2; + double bts4; + double bts6; + double PHIc; + double Qc; + double Qco; + double t; + + + sf = (double)1.0; /* scale factor */ + + phi0 = GPS_Math_Deg_To_Rad(phi0); + M0 = GPS_Math_Deg_To_Rad(M0); + + p2 = (double)GPS_PI * (double)2.0; + po2 = (double)GPS_PI / (double)2.0; + if(M0>GPS_PI) + M0 -= p2; + a2 = a*a; + b2 = b*b; + e2 = (a2-b2)/a2; + e4 = e2*e2; + e6 = e2*e4; + e = pow(e2,(double).5); + om = (double)1.-e2; + som = pow(om,(double).5); + oo = (double)1./((double)2.*e); + + phis = sin(po2); + x = e * phis; + qp = om*(phis/((double)1.-e2*phis*phis)-oo* + log(((double)1.-x)/((double)1.+x))); + + A0 = e2 / (double)3.+(double)31.*e4/(double)180.+(double)517.* + e6/(double)5040.; + A1 = (double)23.*e4/(double)360.+(double)251.*e6/(double)3780.; + A2 = (double)761.*e6/(double)45360.; + + E1 = ((double)1.0-som) / ((double)1.0+som); + E2 = E1*E1; + E3 = E2*E1; + E4 = E3*E1; + + b0 = (double)3.*E1/(double)2.-(double)27.*E3/(double)32.; + b1 = (double)21.*E2/(double)16.-(double)55.*E4/(double)32.; + B2 = (double)151.*E3/(double)96.; + b3 = (double)1097.*E4/(double)512.; + + + j = (double)45.0*e6/(double)1024.0; + te4 = (double)3.0 * e4; + c0 = (double)1.0-e2/(double)4.0-te4/(double)64.0-(double)5.0*e6/ + (double)256.0; + c1 = (double)3.0*e2/(double)8.0+te4/(double)32.0+j; + c2 = (double)15.0*e4/(double)256.0+j; + c3 = (double)35.0*e6/(double)3072.0; + + lat = c0 * phi0; + + phi0s2 = c1 * sin((double)2.0*phi0); + phi0s4 = c2 * sin((double)4.0*phi0); + phi0s6 = c3 * sin((double)6.0*phi0); + AM0 = a*(lat-phi0s2+phi0s4-phi0s6); + + + + dx = E - E0; + dy = N - N0; + Mc = AM0 + dy/sf; + Muc = Mc / (c0*a); + + mus2 = b0 * sin((double)2.0*Muc); + mus4 = b1 * sin((double)4.0*Muc); + mus6 = B2 * sin((double)6.0*Muc); + mus8 = b3 * sin((double)6.0*Muc); + PHIc = Muc + mus2 + mus4 + mus6 + mus8; + + phis = sin(PHIc); + x = e * phis; + Qc = om*(phis/((double)1.-e2*phis*phis)-oo* + log(((double)1.-x)/((double)1.+x))); + Qco = Qc/qp; + + if(Qco>(double)1.) + Qco = (double)1.; + else if(Qco<(double)-1.) + Qco = (double)-1.; + + btc = asin(Qco); + btcc = cos(btc); + t = sf*dx*btcc*pow((double)1.-e2*phis*phis,(double).5)/(a*cos(PHIc)); + if(t>(double)1.) + t=(double)1.; + else if(t<(double)-1.) + t=(double)-1.; + btp = -asin(t); + bt = asin(cos(btp)*sin(btc)); + + bts2 = A0 * sin((double)2.0*bt); + bts4 = A1 * sin((double)4.0*bt); + bts6 = A2 * sin((double)6.0*bt); + *phi = bt + bts2 + bts4 + bts6; + *lambda = M0 - atan(tan(btp)/btcc); + + if(*phi>po2) + *phi = po2; + else if(*phi<-po2) + *phi = -po2; + + if(*lambda>GPS_PI) + *lambda -= p2; + if(*lambda<-GPS_PI) + *lambda += p2; + + if(*lambda>GPS_PI) + *lambda = GPS_PI; + else if(*lambda<-GPS_PI) + *lambda=-GPS_PI; + + *lambda = GPS_Math_Rad_To_Deg(*lambda); + *phi = GPS_Math_Rad_To_Deg(*phi); + + return; +} + + + + +/* @func GPS_Math_Mercator_LatLon_To_EN ********************************** +** +** Convert latitude and longitude to standard Mercator projection +** easting and northing +** +** @param [r] phi [double] latitude (deg) +** @param [r] lambda [double] longitude (deg) +** @param [w] E [double *] easting (metre) +** @param [w] N [double *] northing (metre) +** @param [r] phi0 [double] latitude of origin (deg) +** @param [r] lambda0 [double] longitude of origin (deg) +** @param [r] E0 [double] false easting +** @param [r] N0 [double] false northing +** @param [r] a [double] semi-major axis +** @param [r] b [double] semi-minor axis +** +** @return [void] +************************************************************************/ +void GPS_Math_Mercator_LatLon_To_EN(double phi, double lambda, double *E, + double *N, double phi0, double lambda0, + double E0, double N0, double a, double b) +{ + double p2; + double po2; + double a2; + double b2; + double e2; + double e4; + double e3; + double e; + double es; + double ab; + double bb; + double cb; + double db; + double ml; + double phi0s; + double sf; + + double dlam; + double ct; + double ex; + double tt; + double pt; + + + lambda = GPS_Math_Deg_To_Rad(lambda); + phi = GPS_Math_Deg_To_Rad(phi); + phi0 = GPS_Math_Deg_To_Rad(phi0); + lambda0 = GPS_Math_Deg_To_Rad(lambda0); + + ml = ((double)GPS_PI*(double)89.5)/(double)180.; + + p2 = (double)GPS_PI * (double)2.0; + po2 = (double)GPS_PI / (double)2.0; + if(lambda0>GPS_PI) + lambda0 -= p2; + a2 = a*a; + b2 = b*b; + es = (a2-b2)/a2; + e2 = es*es; + e3 = e2*es; + e4 = e3*es; + + e = pow(es,(double).5); + phi0s = sin(phi0); + sf = (double)1. / (pow((double)1.-es*phi0s*phi0s,(double).5)/cos(phi0)); + + ab = es/(double)2.+(double)5.*e2/(double)24.+e3/(double)12.+(double)13.* + e4/(double)360.; + bb = (double)7.*e2/(double)48.+(double)29.*e3/(double)240.+ + (double)811.*e4/(double)11520.; + cb = (double)7.*e3/(double)120.+(double)81.*e4/(double)1120.; + db = (double)4279.*e4/(double)161280.; + + + + if(lambda>(double)GPS_PI) + lambda -= p2; + + dlam = lambda - lambda0; + if(dlam>GPS_PI) + dlam -= p2; + if(dlam<-GPS_PI) + dlam += p2; + + + ex = e * sin(phi); + tt = tan((double)GPS_PI/(double)4.+phi/(double)2.); + pt = pow((((double)1.-ex)/((double)1.+ex)),(e/(double)2.)); + + ct = tt * pt; + *N = sf * a * log(ct) + N0; + *E = sf * a * dlam + E0; + + return; +} + + + + +/* @func GPS_Math_Mercator_EN_To_LatLon ********************************** +** +** Convert standard Mercator easting and northing projection +** to latitude and longitude +** +** @param [r] E [double] easting (metre) +** @param [r] N [double] northing (metre) +** @param [w] phi [double *] latitude (deg) +** @param [w] lambda [double *] longitude (deg) +** @param [r] phi0 [double] latitude of origin (deg) +** @param [r] lambda0 [double] longitude of origin (deg) +** @param [r] E0 [double] false easting +** @param [r] N0 [double] false northing +** @param [r] a [double] semi-major axis +** @param [r] b [double] semi-minor axis +** +** @return [void] +************************************************************************/ +void GPS_Math_Mercator_EN_To_LatLon(double E, double N, double *phi, + double *lambda, double phi0, + double lambda0, double E0, double N0, + double a, double b) +{ + double p2; + double po2; + double a2; + double b2; + double e2; + double e4; + double e3; + double e; + double es; + double ab; + double bb; + double cb; + double db; + double ml; + double phi0s; + double sf; + + double dx; + double dy; + double x; + + phi0 = GPS_Math_Deg_To_Rad(phi0); + lambda0 = GPS_Math_Deg_To_Rad(lambda0); + + ml = ((double)GPS_PI*(double)89.5)/(double)180.; + + p2 = (double)GPS_PI * (double)2.0; + po2 = (double)GPS_PI / (double)2.0; + if(lambda0>GPS_PI) + lambda0 -= p2; + a2 = a*a; + b2 = b*b; + es = (a2-b2)/a2; + e2 = es*es; + e3 = e2*es; + e4 = e3*es; + + e = pow(es,(double).5); + phi0s = sin(phi0); + sf = (double)1. / (pow((double)1.-es*phi0s*phi0s,(double).5)/cos(phi0)); + + ab = es/(double)2.+(double)5.*e2/(double)24.+e3/(double)12.+(double)13.* + e4/(double)360.; + bb = (double)7.*e2/(double)48.+(double)29.*e3/(double)240.+ + (double)811.*e4/(double)11520.; + cb = (double)7.*e3/(double)120.+(double)81.*e4/(double)1120.; + db = (double)4279.*e4/(double)161280.; + + dx = E - E0; + dy = N - N0; + *lambda = lambda0 + dx / (sf*a); + x = (double)GPS_PI / (double)2. - + (double)2.*atan((double)1./exp(dy/(sf*a))); + *phi = x+ab*sin((double)2.*x)+bb*sin((double)4.*x)+cb*sin((double)6.*x) + + db*sin((double)8.*x); + + if(*phi>po2) + *phi = po2; + else if(*phi<-po2) + *phi = -po2; + + if(*lambda>GPS_PI) + *lambda -= p2; + if(*lambda<-GPS_PI) + *lambda += p2; + + if(*lambda>GPS_PI) + *lambda = GPS_PI; + else if(*lambda<-GPS_PI) + *lambda=-GPS_PI; + + *lambda = GPS_Math_Rad_To_Deg(*lambda); + *phi = GPS_Math_Rad_To_Deg(*phi); + + return; +} + + + + +/* @func GPS_Math_TMerc_LatLon_To_EN ********************************** +** +** Convert latitude and longitude to transverse Mercator projection +** easting and northing +** +** @param [r] phi [double] latitude (deg) +** @param [r] lambda [double] longitude (deg) +** @param [w] E [double *] easting (metre) +** @param [w] N [double *] northing (metre) +** @param [r] phi0 [double] latitude of origin (deg) +** @param [r] lambda0 [double] longitude of origin (deg) +** @param [r] E0 [double] false easting +** @param [r] N0 [double] false northing +** @param [r] F0 [double] scale factor +** @param [r] a [double] semi-major axis +** @param [r] b [double] semi-minor axis +** +** @return [void] +************************************************************************/ +void GPS_Math_TMerc_LatLon_To_EN(double phi, double lambda, double *E, + double *N, double phi0, double lambda0, + double E0, double N0, double F0, + double a, double b) +{ + GPS_Math_LatLon_To_EN(E,N,phi,lambda,N0,E0,phi0,lambda0,F0,a,b); + + return; +} + + + + +/* @func GPS_Math_TMerc_EN_To_LatLon ********************************** +** +** Convert transverse Mercator easting and northing projection +** to latitude and longitude +** +** @param [r] E [double] easting (metre) +** @param [r] N [double] northing (metre) +** @param [w] phi [double *] latitude (deg) +** @param [w] lambda [double *] longitude (deg) +** @param [r] phi0 [double] latitude of origin (deg) +** @param [r] lambda0 [double] longitude of origin (deg) +** @param [r] E0 [double] false easting +** @param [r] N0 [double] false northing +** @param [r] F0 [double] scale factor +** @param [r] a [double] semi-major axis +** @param [r] b [double] semi-minor axis +** +** @return [void] +************************************************************************/ +void GPS_Math_TMerc_EN_To_LatLon(double E, double N, double *phi, + double *lambda, double phi0, double lambda0, + double E0, double N0, double F0, + double a, double b) +{ + GPS_Math_EN_To_LatLon(E,N,phi,lambda,N0,E0,phi0,lambda0,F0,a,b); + + return; +} + + + + +/* @func GPS_Math_Swiss_LatLon_To_EN *********************************** +** +** Convert latitude and longitude to Swiss grid easting and northing +** +** @param [r] phi [double] latitude (deg) +** @param [r] lambda [double] longitude (deg) +** @param [w] E [double *] easting (metre) +** @param [w] N [double *] northing (metre) +** @param [r] phi0 [double] latitude origin (deg) [normally 46.95240556] +** @param [r] lambda0 [double] longitude origin (deg) [normally 7.43958333] +** @param [r] E0 [double] false easting (metre) [normally 600000.0] +** @param [r] N0 [double] false northing (metre) [normally 200000.0] +** @param [r] a [double] semi-major axis [normally 6377397.000] +** @param [r] b [double] semi-minor axis [normally 6356078.823] +** +** @return [void] +***************************************************************************/ +void GPS_Math_Swiss_LatLon_To_EN(double phi, double lambda, double *E, + double *N,double phi0,double lambda0, + double E0, double N0, double a, double b) + +{ + double a2; + double b2; + double esq; + double e; + double c; + double ephi0p; + double phip; + double sphip; + double phid; + double slambda2; + double lambda1; + double lambda2; + double K; + double po4; + double w; + double R; + + lambda0 = GPS_Math_Deg_To_Rad(lambda0); + phi0 = GPS_Math_Deg_To_Rad(phi0); + lambda = GPS_Math_Deg_To_Rad(lambda); + phi = GPS_Math_Deg_To_Rad(phi); + + po4=GPS_PI/(double)4.0; + + a2 = a*a; + b2 = b*b; + esq = (a2-b2)/a2; + e = pow(esq,(double)0.5); + + c = sqrt(1+((esq*pow(cos(phi0),(double)4.))/((double)1.-esq))); + + ephi0p = asin(sin(phi0)/c); + + K = log(tan(po4+ephi0p/(double)2.)) - c*(log(tan(po4+phi0/(double)2.)) - + e/(double)2. * log(((double)1.+e*sin(phi0)) / + ((double)1.-e*sin(phi0)))); + lambda1 = c*(lambda-lambda0); + w = c*(log(tan(po4+phi/(double)2.)) - e/(double)2. * + log(((double)1.+e*sin(phi)) / ((double)1.-e*sin(phi)))) + K; + + + phip = (double)2. * (atan(exp(w)) - po4); + + sphip = cos(ephi0p) * sin(phip) - sin(ephi0p) * cos(phip) * cos(lambda1); + phid = asin(sphip); + + slambda2 = cos(phip)*sin(lambda1) / cos(phid); + lambda2 = asin(slambda2); + + R = a*sqrt((double)1.-esq) / ((double)1.-esq*sin(phi0) * sin(phi0)); + + *N = R*log(tan(po4 + phid/(double)2.)) + N0; + *E = R*lambda2 + E0; + return; +} + + + + +/* @func GPS_Math_Swiss_EN_To_LatLon ************************************ +** +** Convert Swiss Grid easting and northing to latitude and longitude +** +** @param [r] E [double] easting (metre) +** @param [r] N [double] northing (metre) +** @param [w] phi [double *] latitude (deg) +** @param [w] lambda [double *] longitude (deg) +** @param [r] phi0 [double] latitude origin (deg) [normally 46.95240556] +** @param [r] lambda0 [double] longitude origin (deg) [normally 7.43958333] +** @param [r] E0 [double] false easting (metre) [normally 600000.0] +** @param [r] N0 [double] false northing (metre) [normally 200000.0] +** @param [r] a [double] semi-major axis [normally 6377397.000] +** @param [r] b [double] semi-minor axis [normally 6356078.823] +** +** @return [void] +*************************************************************************/ + +void GPS_Math_Swiss_EN_To_LatLon(double E, double N, double *phi, + double *lambda, double phi0, double lambda0, + double E0, double N0, double a, double b) +{ + double a2; + double b2; + double esq; + double e; + double R; + double c; + double po4; + double phid; + double phi1; + double lambdad; + double lambda1; + double slambda1; + double ephi0p; + double sphip; + double tol; + double cr; + double C; + double K; + + lambda0 = GPS_Math_Deg_To_Rad(lambda0); + phi0 = GPS_Math_Deg_To_Rad(phi0); + + po4=GPS_PI/(double)4.0; + tol=(double)0.00001; + + a2 = a*a; + b2 = b*b; + esq = (a2-b2)/a2; + e = pow(esq,(double)0.5); + + R = a*sqrt((double)1.-esq) / ((double)1.-esq*sin(phi0) * sin(phi0)); + + phid = (double)2.*(atan(exp((N - N0)/R)) - po4); + lambdad = (E - E0)/R; + + c = sqrt((double)1.+((esq * pow(cos(phi0), (double)4.)) / + ((double)1.-esq))); + ephi0p = asin(sin(phi0) / c); + + sphip = cos(ephi0p)*sin(phid) + sin(ephi0p)*cos(phid)*cos(lambdad); + phi1 = asin(sphip); + + slambda1 = cos(phid)*sin(lambdad)/cos(phi1); + lambda1 = asin(slambda1); + + *lambda = GPS_Math_Rad_To_Deg((lambda1/c + lambda0)); + + K = log(tan(po4 + ephi0p/(double)2.)) -c*(log(tan(po4 + phi0/(double)2.)) + - e/(double)2. * log(((double)1.+e*sin(phi0)) / + ((double)1.-e*sin(phi0)))); + C = (K - log(tan(po4 + phi1/(double)2.)))/c; + + do + { + cr = (C + log(tan(po4 + phi1/(double)2.)) - e/(double)2. * + log(((double)1.+e*sin(phi1)) / ((double)1.-e*sin(phi1)))) * + ((((double)1.-esq*sin(phi1)*sin(phi1)) * cos(phi1)) / + ((double)1.-esq)); + phi1 -= cr; + } + while (fabs(cr) > tol); + + *phi = GPS_Math_Rad_To_Deg(phi1); + + return; +} diff --git a/gpsbabel/jeeps/gpsproj.h b/gpsbabel/jeeps/gpsproj.h new file mode 100644 index 000000000..6922a47e0 --- /dev/null +++ b/gpsbabel/jeeps/gpsproj.h @@ -0,0 +1,157 @@ +#ifdef __cplusplus +extern "C" +{ +#endif + +#ifndef gpsproj_h +#define gpsproj_h + + +#include "gps.h" + +void GPS_Math_Albers_LatLon_To_EN(double phi, double lambda, double *E, + double *N, double phi1, double phi2, + double phi0, double M0, double E0, + double N0, double a, double b); +void GPS_Math_Albers_EN_To_LatLon(double E, double N, double *phi, + double *lambda, double phi1, double phi2, + double phi0, double M0, double E0, + double N0, double a, double b); + + +void GPS_Math_LambertCC_LatLon_To_EN(double phi, double lambda, double *E, + double *N, double phi1, double phi2, + double phi0, double M0, double E0, + double N0, double a, double b); +void GPS_Math_LambertCC_EN_To_LatLon(double E, double N, double *phi, + double *lambda, double phi1, double phi2, + double phi0, double M0, double E0, + double N0, double a, double b); + +void GPS_Math_Miller_LatLon_To_EN(double phi, double lambda, double *E, + double *N, double M0, double E0, + double N0, double a, double b); +void GPS_Math_Miller_EN_To_LatLon(double E, double N, double *phi, + double *lambda, double M0, double E0, + double N0, double a, double b); + +void GPS_Math_Bonne_LatLon_To_EN(double phi, double lambda, double *E, + double *N, double phi0, double M0, double E0, + double N0, double a, double b); +void GPS_Math_Bonne_EN_To_LatLon(double E, double N, double *phi, + double *lambda, double phi0, double M0, + double E0, double N0, double a, double b); + +void GPS_Math_Cassini_LatLon_To_EN(double phi, double lambda, double *E, + double *N, double phi0, double M0, + double E0, double N0, double a, double b); +void GPS_Math_Cassini_EN_To_LatLon(double E, double N, double *phi, + double *lambda, double phi0, double M0, + double E0, double N0, double a, double b); + +void GPS_Math_Cylea_LatLon_To_EN(double phi, double lambda, double *E, + double *N, double phi0, double M0, + double E0, double N0, double a, double b); +void GPS_Math_Cylea_EN_To_LatLon(double E, double N, double *phi, + double *lambda, double phi0, double M0, + double E0, double N0, double a, double b); + +void GPS_Math_EckertIV_LatLon_To_EN(double phi, double lambda, double *E, + double *N, double M0, double E0, double N0, + double a, double b); +void GPS_Math_EckertIV_EN_To_LatLon(double E, double N, double *phi, + double *lambda, double M0, double E0, + double N0, double a, double b); + +void GPS_Math_EckertVI_LatLon_To_EN(double phi, double lambda, double *E, + double *N, double M0, double E0, double N0, + double a, double b); +void GPS_Math_EckertVI_EN_To_LatLon(double E, double N, double *phi, + double *lambda, double M0, double E0, + double N0, double a, double b); + +void GPS_Math_Cyled_LatLon_To_EN(double phi, double lambda, double *E, + double *N, double phi0, double M0, double E0, + double N0, double a, double b); +void GPS_Math_Cyled_EN_To_LatLon(double E, double N, double *phi, + double *lambda, double phi0, double M0, + double E0, double N0, double a, double b); + +void GPS_Math_VderGrinten_LatLon_To_EN(double phi, double lambda, double *E, + double *N, double M0, double E0, + double N0, double a, double b); +void GPS_Math_VderGrinten_EN_To_LatLon(double E, double N, double *phi, + double *lambda, double M0, double E0, + double N0, double a, double b); + +void GPS_Math_PolarSt_LatLon_To_EN(double phi, double lambda, double *E, + double *N, double phi1, double lambda1, + double E0, double N0, double a, double b); +void GPS_Math_PolarSt_EN_To_LatLon(double E, double N, double *phi, + double *lambda, double phi1, double lambda1, + double E0, double N0, double a, double b); + +void GPS_Math_Mollweide_LatLon_To_EN(double phi, double lambda, double *E, + double *N, double M0, double E0, + double N0, double a, double b); +void GPS_Math_Mollweide_EN_To_LatLon(double E, double N, double *phi, + double *lambda, double M0, double E0, + double N0, double a, double b); + +void GPS_Math_Orthog_LatLon_To_EN(double phi, double lambda, double *E, + double *N, double phi0, double lambda0, + double E0, double N0, double a, double b); +void GPS_Math_Orthog_EN_To_LatLon(double E, double N, double *phi, + double *lambda, double phi0, double lambda0, + double E0, double N0, double a, double b); + +void GPS_Math_Polycon_LatLon_To_EN(double phi, double lambda, double *E, + double *N, double phi0, double M0, + double E0, double N0, double a, double b); +void GPS_Math_Polycon_EN_To_LatLon(double E, double N, double *phi, + double *lambda, double phi0, double M0, + double E0, double N0, double a, double b); + +void GPS_Math_Sinusoid_LatLon_To_EN(double phi, double lambda, double *E, + double *N, double M0, double E0, + double N0, double a, double b); +void GPS_Math_Sinusoid_EN_To_LatLon(double E, double N, double *phi, + double *lambda, double M0, double E0, + double N0, double a, double b); + +void GPS_Math_TCylEA_LatLon_To_EN(double phi, double lambda, double *E, + double *N, double phi0, double M0, double E0, + double N0, double a, double b); +void GPS_Math_TCylEA_EN_To_LatLon(double E, double N, double *phi, + double *lambda, double phi0, double M0, + double E0, double N0, double a, double b); + +void GPS_Math_Mercator_LatLon_To_EN(double phi, double lambda, double *E, + double *N, double phi0, double lambda0, + double E0, double N0, double a, double b); +void GPS_Math_Mercator_EN_To_LatLon(double E, double N, double *phi, + double *lambda, double phi0, + double lambda0, double E0, double N0, + double a, double b); + +void GPS_Math_TMerc_LatLon_To_EN(double phi, double lambda, double *E, + double *N, double phi0, double lambda0, + double E0, double N0, double F0, + double a, double b); +void GPS_Math_TMerc_EN_To_LatLon(double E, double N, double *phi, + double *lambda, double phi0, double lambda0, + double E0, double N0, double F0, + double a, double b); + +void GPS_Math_Swiss_LatLon_To_EN(double phi, double lambda, double *E, + double *N,double phi0,double lambda0, + double E0, double N0, double a, double b); +void GPS_Math_Swiss_EN_To_LatLon(double E, double N, double *phi, + double *lambda, double phi0, double lambda0, + double E0, double N0, double a, double b); + +#endif + +#ifdef __cplusplus +} +#endif diff --git a/gpsbabel/jeeps/gpsprot.c b/gpsbabel/jeeps/gpsprot.c new file mode 100644 index 000000000..50606b7d2 --- /dev/null +++ b/gpsbabel/jeeps/gpsprot.c @@ -0,0 +1,366 @@ +/******************************************************************** +** @source JEEPS protocol table lookup functions (GPS' without A001) +** +** @author Copyright (C) 1999 Alan Bleasby +** @version 1.0 +** @modified Dec 28 1999 Alan Bleasby. First version +** @@ +** +** This library is free software; you can redistribute it and/or +** modify it under the terms of the GNU Library General Public +** License as published by the Free Software Foundation; either +** version 2 of the License, or (at your option) any later version. +** +** This library is distributed in the hope that it will be useful, +** but WITHOUT ANY WARRANTY; without even the implied warranty of +** MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU +** Library General Public License for more details. +** +** You should have received a copy of the GNU Library General Public +** License along with this library; if not, write to the +** Free Software Foundation, Inc., 59 Temple Place - Suite 330, +** Boston, MA 02111-1307, USA. +********************************************************************/ +#include "gps.h" +#include + +#define GPS_TAGUNK 20 + +/* Storage for any unknown tags */ +static int32 gps_tag_unknown[GPS_TAGUNK]; +static int32 gps_tag_data_unknown[GPS_TAGUNK]; +static int32 gps_n_tag_unknown = 0; + + + +struct COMMANDDATA COMMAND_ID[2]= +{ + { + 0x00,0x01,0x02,0x03,0x04,0x05,0x06,0x07,0x08,0x31,0x32 + } + , + { + 0x00,0x04,0x00,0x00,0x08,0x14,0x00,0x15,0x1a,0x00,0x00 + } +}; + +struct LINKDATA LINK_ID[3]= +{ + { + 0x06,0,0,0,0,0,0x15,0,0,0,0,0,0,0,0,0,0xfd,0xfe,0xff + } + , + { + 0x06,0x0a,0x0c,0x0e,0x11,0x13,0x15,0x1b,0x1d,0x1e, + 0x1f,0x22,0x23,0x33,0x62,0x63,0xfd,0xfe,0xff + } + , + { + 0x06,0x0b,0x0c,0x14,0x18,0,0x15,0x23,0x25,0x27,0x04, + 0,0x2b,0,0,0,0xfd,0xfe,0xff + } +}; + +struct GPS_MODEL_PROTOCOL GPS_MP[]= +{ + { 7,pL001,pA010,pA100,pD100,pA200,pD200,-1,-1,-1,-1, + pA500,pD500 + }, + { 13,pL001,pA010,pA100,pD100,pA200,pD200,pD100,pA300,pD300,pA400,pD400, + pA500,pD500 + }, + { 14,pL001,pA010,pA100,pD100,pA200,pD200,pD100,-1,-1,pA400,pD400, + pA500,pD500 + }, + { 15,pL001,pA010,pA100,pD151,pA200,pD200,pD151,-1,-1,pA400,pD151, + pA500,pD500 + }, + { 18,pL001,pA010,pA100,pD100,pA200,pD200,pD100,pA300,pD300,pA400,pD400, + pA500,pD500 + }, + { 20,pL002,pA011,pA100,pD150,pA200,pD201,pD150,-1,-1,pA400,pD450, + pA500,pD550 + }, + { 22,pL001,pA010,pA100,pD152,pA200,pD200,pD152,pA300,pD300,pA400,pD152, + pA500,pD500 + }, + { 23,pL001,pA010,pA100,pD100,pA200,pD200,pD100,pA300,pD300,pA400,pD400, + pA500,pD500 + }, + { 24,pL001,pA010,pA100,pD100,pA200,pD200,pD100,pA300,pD300,pA400,pD400, + pA500,pD500 + }, + { 25,pL001,pA010,pA100,pD100,pA200,pD200,pD100,pA300,pD300,pA400,pD400, + pA500,pD500 + }, + { 29,pL001,pA010,pA100,pD101,pA200,pD201,pD101,pA300,pD300,pA400,pD101, + pA500,pD500 + }, + { 929,pL001,pA010,pA100,pD102,pA200,pD201,pD102,pA300,pD300,pA400,pD102, + pA500,pD500 + }, + { 31,pL001,pA010,pA100,pD100,pA200,pD201,pD100,pA300,pD300,-1,-1, + pA500,pD500 + }, + { 33,pL002,pA011,pA100,pD150,pA200,pD201,pD150,-1,-1,pA400,pD450, + pA500,pD550 + }, + { 34,pL002,pA011,pA100,pD150,pA200,pD201,pD150,-1,-1,pA400,pD450, + pA500,pD550 + }, + { 35,pL001,pA010,pA100,pD100,pA200,pD200,pD100,pA300,pD300,pA400,pD400, + pA500,pD500 + }, + { 36,pL001,pA010,pA100,pD152,pA200,pD200,pD152,pA300,pD300,pA400,pD152, + pA500,pD500 + }, + { 936,pL001,pA010,pA100,pD152,pA200,pD200,pD152,pA300,pD300,-1,-1, + pA500,pD500 + }, + { 39,pL001,pA010,pA100,pD151,pA200,pD201,pD151,pA300,pD300,-1,-1, + pA500,pD500 + }, + { 41,pL001,pA010,pA100,pD100,pA200,pD201,pD100,pA300,pD300,-1,-1, + pA500,pD500 + }, + { 42,pL001,pA010,pA100,pD100,pA200,pD200,pD100,pA300,pD300,pA400,pD400, + pA500,pD500 + }, + { 44,pL001,pA010,pA100,pD101,pA200,pD201,pD101,pA300,pD300,pA400,pD101, + pA500,pD500 + }, + { 45,pL001,pA010,pA100,pD152,pA200,pD201,pD152,pA300,pD300,-1,-1, + pA500,pD500 + }, + { 47,pL001,pA010,pA100,pD100,pA200,pD201,pD100,pA300,pD300,-1,-1, + pA500,pD500 + }, + { 48,pL001,pA010,pA100,pD154,pA200,pD201,pD154,pA300,pD300,-1,-1, + pA500,pD501 + }, + { 49,pL001,pA010,pA100,pD102,pA200,pD201,pD102,pA300,pD300,pA400,pD102, + pA500,pD501 + }, + { 50,pL001,pA010,pA100,pD152,pA200,pD201,pD152,pA300,pD300,-1,-1, + pA500,pD501 + }, + { 52,pL002,pA011,pA100,pD150,pA200,pD201,pD150,-1,-1,pA400,pD450, + pA500,pD550 + }, + { 53,pL001,pA010,pA100,pD152,pA200,pD201,pD152,pA300,pD300,-1,-1, + pA500,pD501 + }, + { 55,pL001,pA010,pA100,pD100,pA200,pD201,pD100,pA300,pD300,-1,-1, + pA500,pD500 + }, + { 56,pL001,pA010,pA100,pD100,pA200,pD201,pD100,pA300,pD300,-1,-1, + pA500,pD500 + }, + { 59,pL001,pA010,pA100,pD100,pA200,pD201,pD100,pA300,pD300,-1,-1, + pA500,pD500 + }, + { 61,pL001,pA010,pA100,pD100,pA200,pD201,pD100,pA300,pD300,-1,-1, + pA500,pD500 + }, + { 62,pL001,pA010,pA100,pD100,pA200,pD201,pD100,pA300,pD300,-1,-1, + pA500,pD500 + }, + { 64,pL002,pA011,pA100,pD150,pA200,pD201,pD150,-1,-1,pA400,pD450, + pA500,pD551 + }, + { 71,pL001,pA010,pA100,pD155,pA200,pD201,pD155,pA300,pD300,-1,-1, + pA500,pD501 + }, + { 72,pL001,pA010,pA100,pD104,pA200,pD201,pD104,pA300,pD300,-1,-1, + pA500,pD501 + }, + { 73,pL001,pA010,pA100,pD103,pA200,pD201,pD103,pA300,pD300,-1,-1, + pA500,pD501 + }, + { 74,pL001,pA010,pA100,pD100,pA200,pD201,pD100,pA300,pD300,-1,-1, + pA500,pD500 + }, + { 76,pL001,pA010,pA100,pD102,pA200,pD201,pD102,pA300,pD300,pA400,pD102, + pA500,pD501 + }, + { 77,pL001,pA010,pA100,pD100,pA200,pD201,pD100,pA300,pD300,pA400,pD400, + pA500,pD501 + }, + { 777,pL001,pA010,pA100,pD103,pA200,pD201,pD103,pA300,pD300,pA400,pD403, + pA500,pD501 + }, + { 877,pL001,pA010,pA100,pD103,pA200,pD201,pD103,pA300,pD300,-1,-1, + pA500,pD501 + }, + { 977,pL001,pA010,pA100,pD103,pA200,pD201,pD103,pA300,pD300,pA400,pD403, + pA500,pD501 + }, + { 87,pL001,pA010,pA100,pD103,pA200,pD201,pD103,pA300,pD300,pA400,pD403, + pA500,pD501 + }, + { 88,pL001,pA010,pA100,pD102,pA200,pD201,pD102,pA300,pD300,pA400,pD102, + pA500,pD501 + }, + { 95,pL001,pA010,pA100,pD103,pA200,pD201,pD103,pA300,pD300,pA400,pD403, + pA500,pD501 + }, + { 96,pL001,pA010,pA100,pD103,pA200,pD201,pD103,pA300,pD300,pA400,pD403, + pA500,pD501 + }, + { 97,pL001,pA010,pA100,pD103,pA200,pD201,pD103,pA300,pD300,-1,-1, + pA500,pD501 + }, + { 98,pL002,pA011,pA100,pD150,pA200,pD201,pD150,-1,-1,pA400,pD450, + pA500,pD551 + }, + { 100,pL001,pA010,pA100,pD103,pA200,pD201,pD103,pA300,pD300,pA400,pD403, + pA500,pD501 + }, + { 105,pL001,pA010,pA100,pD103,pA200,pD201,pD103,pA300,pD300,pA400,pD403, + pA500,pD501 + }, + { 106,pL001,pA010,pA100,pD103,pA200,pD201,pD103,pA300,pD300,pA400,pD403, + pA500,pD501 + }, + { 112,pL001,pA010,pA100,pD152,pA200,pD201,pD152,pA300,pD300,-1,-1, + pA500,pD501 + }, + { 0,0,0,0,0,0,0,0,0,0,0,0,0,0 + } +}; + + +/* @func GPS_Protocol_Version_Change ************************************ +** +** Alters/recalculates ID, if necessary, for indexing the GPS_MP +** structure in order to find available protocols. +** +** @param [r] id [US] Garmin id +** @param [r] version [UD] Garmin version +** +** @return [void] +************************************************************************/ + +US GPS_Protocol_Version_Change(US id, US version) +{ + if(id==29) + if(version>=400) + id = 929; + + if(id==36) + if(version>=300) + id = 936; + + if(id==77) + { + if(version>=301 && version<350) + id = 777; + else if(version>=350 && version<361) + id = 877; + else if(version>=361) + id = 977; + } + + return id; +} + + + +/* @func GPS_Protocol_Table_Set ************************************ +** +** Set protocol capabilities based on table look-up +** For those units without the A001 protocol +** +** @param [r] id [const US] id +** +** @return [int32] Success +************************************************************************/ + +int32 GPS_Protocol_Table_Set(US id) +{ + int32 i; + US v; + char s[GPS_ARB_LEN]; + + i=0; + while((v=GPS_MP[i].id)) + { + if(v==id) + { + gps_link_type = GPS_MP[i].link; + gps_device_command = GPS_MP[i].command-10; + gps_waypt_transfer = GPS_MP[i].wayptt; + gps_waypt_type = GPS_MP[i].wayptd; + gps_route_transfer = GPS_MP[i].rtea; + gps_rte_hdr_type = GPS_MP[i].rted0; + gps_rte_type = GPS_MP[i].rted1; + gps_trk_transfer = GPS_MP[i].trka; + gps_trk_type = GPS_MP[i].trkd; + gps_prx_waypt_transfer = GPS_MP[i].prxa; + gps_prx_waypt_type = GPS_MP[i].prxd; + gps_almanac_transfer = GPS_MP[i].alma; + gps_almanac_type = GPS_MP[i].almd; + return 1; + } + ++i; + } + + + (void)sprintf(s,"INIT: No table entry for ID %d\n",id); + GPS_Error(s); + + return GPS_UNSUPPORTED; +} + + +/* @func GPS_Protocol_Error ******************************************* +** +** Called if an unrecognised/illegal protocol is met +** For those units with the A001 protocol +** +** @param [r] tag [const US] tag +** @param [r] data [const US] data +** +** @return [void] +************************************************************************/ + +void GPS_Protocol_Error(US tag, US data) +{ + char s[GPS_ARB_LEN]; + + (void) sprintf(s,"PROTOCOL ERROR: Unknown tag/data [%c/%d]\n",tag,data); + GPS_Error(s); + + if(gps_n_tag_unknown < GPS_TAGUNK) + { + gps_tag_unknown[gps_n_tag_unknown] = tag; + gps_tag_data_unknown[gps_n_tag_unknown++] = data; + } + + return; +} + + + +/* @func GPS_Unknown_Protocol_Print ******************************************* +** +** Diagnostic routine for printing out any unknown protocols +** For those units with the A001 protocol +** +** @return [void] +************************************************************************/ + +void GPS_Unknown_Protocol_Print(void) +{ + int32 i; + + (void) fprintf(stdout,"\nUnknown protocols: "); + if(!gps_n_tag_unknown) + (void) fprintf(stdout,"None"); + (void) fprintf(stdout,"\n"); + + for(i=0; i +#include +#include +#include +#include + + +/* @func GPS_Time_Now *********************************************** +** +** Get current time +** +** @return [time_t] number of bytes read +**********************************************************************/ + +time_t GPS_Time_Now(void) +{ + time_t secs; + + if(time(&secs)==-1) + { + perror("time"); + GPS_Error("GPS_Time_Now: Error reading time"); + gps_errno = HARDWARE_ERROR; + return 0; + } + + return secs; +} + + + + + + + +/* @func GPS_Packet_Read *********************************************** +** +** Read a packet +** +** @param [r] fd [int32] file descriptor +** @param [w] packet [GPS_PPacket *] packet string +** +** @return [int32] number of bytes read +**********************************************************************/ + +int32 GPS_Packet_Read(int32 fd, GPS_PPacket *packet) +{ + time_t start; + int32 n; + int32 len; + UC u; + int32 isDLE; + UC *p; + int32 i; + UC chk=0; + + len = 0; + isDLE = gpsFalse; + p = (*packet)->data; + + start = GPS_Time_Now(); + while(GPS_Time_Now() < start+GPS_TIME_OUT) + { + if((n=GPS_Serial_Chars_Ready(fd))) + { + if(GPS_Serial_Read(fd,&u,1)==-1) + { + perror("read"); + GPS_Error("GPS_Packet_Read: Read error"); + gps_errno = FRAMING_ERROR; + return 0; + } + + GPS_Diagnose(u); + + if(!len) + { + (*packet)->dle = u; + if(u != DLE) + { + (void) fprintf(stderr,"GPS_Packet_Read: No DLE\n"); + (void) fflush(stderr); + return 0; + } + ++len; + continue; + } + + if(len==1) + { + (*packet)->type = u; + ++len; + continue; + } + + if(u == DLE) + { + if(isDLE) + { + isDLE = gpsFalse; + continue; + } + isDLE = gpsTrue; + } + + if(len == 2) + { + (*packet)->n = u; + len = -1; + continue; + } + + if(u == ETX) + if(isDLE) + { + (*packet)->edle = DLE; + (*packet)->etx = ETX; + if(p-(*packet)->data-2 != (*packet)->n) + { + GPS_Error("GPS_Packet_Read: Bad count"); + gps_errno = FRAMING_ERROR; + return 0; + } + (*packet)->chk = *(p-2); + + for(i=0,p=(*packet)->data;i<(*packet)->n;++i) + chk -= *p++; + chk -= (*packet)->type; + chk -= (*packet)->n; + if(chk != (*packet)->chk) + { + GPS_Error("CHECKSUM: Read error\n"); + gps_errno = FRAMING_ERROR; + return 0; + } + + return (*packet)->n; + } + + *p++ = u; + } + } + + + GPS_Error("GPS_Packet_Read: Time-out"); + gps_errno = SERIAL_ERROR; + + return 0; +} + + + +/* @func GPS_Get_Ack ************************************************* +** +** Check that returned packet is an ack for the packet sent +** +** @param [r] fd [int32] file descriptor +** @param [r] tra [GPS_PPacket *] packet just transmitted +** @param [r] rec [GPS_PPacket *] packet to receive +** +** @return [int32] true if ACK +**********************************************************************/ + +int32 GPS_Get_Ack(int32 fd, GPS_PPacket *tra, GPS_PPacket *rec) +{ + if(!GPS_Packet_Read(fd, rec)) + return 0; + + if(LINK_ID[0].Pid_Ack_Byte != (*rec)->type) + { + gps_error = FRAMING_ERROR; + return 0; + } + + if(*(*rec)->data != (*tra)->type) + { + gps_error = FRAMING_ERROR; + return 0; + } + + return 1; +} + + + + + +#if 0 + +int32 ajb(int32 fd) +{ + UC u; + int32 n; + + while(1) + { + if((n=GPS_Serial_Chars_Ready(fd))) + { + if(read(fd,&u,1)==-1) + { + perror("read"); + GPS_Error("NMEA Read: Read error"); + gps_errno = FRAMING_ERROR; + return 0; + } + printf("%d %c\n",u,u); + } + } + + return 0; +} +#endif diff --git a/gpsbabel/jeeps/gpsread.h b/gpsbabel/jeeps/gpsread.h new file mode 100644 index 000000000..138cc5866 --- /dev/null +++ b/gpsbabel/jeeps/gpsread.h @@ -0,0 +1,23 @@ +#ifdef __cplusplus +extern "C" +{ +#endif + +#ifndef gpsread_h +#define gpsread_h + + +#include "gps.h" +#include + +time_t GPS_Time_Now(void); +int32 GPS_Packet_Read(int32 fd, GPS_PPacket *packet); +int32 GPS_Get_Ack(int32 fd, GPS_PPacket *tra, GPS_PPacket *rec); +int32 ajb(int32 fd); + + +#endif + +#ifdef __cplusplus +} +#endif diff --git a/gpsbabel/jeeps/gpsrqst.c b/gpsbabel/jeeps/gpsrqst.c new file mode 100644 index 000000000..827559071 --- /dev/null +++ b/gpsbabel/jeeps/gpsrqst.c @@ -0,0 +1,175 @@ +/******************************************************************** +** @source JEEPS time/position request from GPS functions +** +** @author Copyright (C) 1999 Alan Bleasby +** @version 1.0 +** @modified Dec 28 1999 Alan Bleasby. First version +** @@ +** +** This library is free software; you can redistribute it and/or +** modify it under the terms of the GNU Library General Public +** License as published by the Free Software Foundation; either +** version 2 of the License, or (at your option) any later version. +** +** This library is distributed in the hope that it will be useful, +** but WITHOUT ANY WARRANTY; without even the implied warranty of +** MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU +** Library General Public License for more details. +** +** You should have received a copy of the GNU Library General Public +** License along with this library; if not, write to the +** Free Software Foundation, Inc., 59 Temple Place - Suite 330, +** Boston, MA 02111-1307, USA. +********************************************************************/ +#include "gps.h" + + +static int32 GPS_A600_Rqst(int32 fd, time_t Time); +static int32 GPS_A700_Rqst(int32 fd, double lat, double lon); + + + +/* @func GPS_Rqst_Send_Time ****************************************** +** +** Set GPS time on request of GPS +** +** @param [r] fd [int32] file descriptor +** @param [r] Time [time_t] unix-style time +** +** @return [int32] true if OK +************************************************************************/ + +int32 GPS_Rqst_Send_Time(int32 fd, time_t Time) +{ + time_t ret=0; + + switch(gps_date_time_transfer) + { + case pA600: + ret = GPS_A600_Rqst(fd, Time); + break; + default: + GPS_Error("Rqst_Send_Time: Unknown date/time protocol"); + return PROTOCOL_ERROR; + } + + return ret; +} + + + +/* @funcstatic GPS_A600_Rqst ******************************************* +** +** Send time to GPS +** +** @param [r] fd [int32] file descriptor +** @param [r] Time [time_t] unix-style time +** +** @return [int32] success +************************************************************************/ +static int32 GPS_A600_Rqst(int32 fd, time_t Time) +{ + GPS_PPacket tra; + GPS_PPacket rec; + + if(!(tra = GPS_Packet_New()) || !(rec = GPS_Packet_New())) + return MEMORY_ERROR; + + + switch(gps_date_time_type) + { + case pD600: + GPS_D600_Send(&tra,Time); + break; + default: + GPS_Error("A600_Rqst: Unknown data/time protocol"); + return PROTOCOL_ERROR; + } + + if(!GPS_Write_Packet(fd,tra)) + return gps_errno; + if(!GPS_Get_Ack(fd, &tra, &rec)) + return gps_errno; + + GPS_Packet_Del(&tra); + GPS_Packet_Del(&rec); + + return 1; +} + + + +/* @func GPS_Rqst_Send_Position ****************************************** +** +** Set GPS position +** +** @param [r] fd [int32] filedescriptor +** @param [r] lat [double] latitude (deg) +** @param [r] lon [double] longitude (deg) +** +** @return [int32] success +************************************************************************/ + +int32 GPS_Rqst_Send_Position(int32 fd, double lat, double lon) +{ + int32 ret=0; + + switch(gps_position_transfer) + { + case pA700: + ret = GPS_A700_Rqst(fd, lat, lon); + break; + default: + GPS_Error("Rqst_Send_Position: Unknown position protocol"); + return PROTOCOL_ERROR; + } + + return ret; +} + + + +/* @funcstatic GPS_A700_Rqst ******************************************* +** +** Send position to GPS +** +** @param [r] fd [int32] file descriptor +** @param [r] lat [double] latitude (deg) +** @param [r] lon [double] longitute (deg) +** +** @return [int32] success +************************************************************************/ +static int32 GPS_A700_Rqst(int32 fd, double lat, double lon) +{ + GPS_PPacket tra; + GPS_PPacket rec; + + if(!(tra = GPS_Packet_New()) || !(rec = GPS_Packet_New())) + return MEMORY_ERROR; + + + switch(gps_position_type) + { + case pD700: + GPS_D700_Send(&tra,lat,lon); + break; + default: + GPS_Error("A700_Rqst: Unknown position protocol"); + GPS_Packet_Del(&tra); + GPS_Packet_Del(&rec); + return PROTOCOL_ERROR; + } + + if(!GPS_Write_Packet(fd,tra)) + return gps_errno; + + if(!GPS_Get_Ack(fd, &tra, &rec)) + return gps_errno; + + + GPS_Packet_Del(&tra); + GPS_Packet_Del(&rec); + + return 1; +} + diff --git a/gpsbabel/jeeps/gpsrqst.h b/gpsbabel/jeeps/gpsrqst.h new file mode 100644 index 000000000..c6398ecd0 --- /dev/null +++ b/gpsbabel/jeeps/gpsrqst.h @@ -0,0 +1,20 @@ +#ifdef __cplusplus +extern "C" +{ +#endif + +#ifndef gpsrqst_h +#define gpsrqst_h + + +#include "gps.h" + +int32 GPS_Rqst_Send_Time(int32 fd, time_t Time); +int32 GPS_Rqst_Send_Position(int32 fd, double lat, double lon); + + +#endif + +#ifdef __cplusplus +} +#endif diff --git a/gpsbabel/jeeps/gpssend.c b/gpsbabel/jeeps/gpssend.c new file mode 100644 index 000000000..372a5dfe9 --- /dev/null +++ b/gpsbabel/jeeps/gpssend.c @@ -0,0 +1,178 @@ +/******************************************************************** +** @source JEEPS packet construction, sending and ack functions +** +** @author Copyright (C) 1999 Alan Bleasby +** @version 1.0 +** @modified Dec 28 1999 Alan Bleasby. First version +** @@ +** +** This library is free software; you can redistribute it and/or +** modify it under the terms of the GNU Library General Public +** License as published by the Free Software Foundation; either +** version 2 of the License, or (at your option) any later version. +** +** This library is distributed in the hope that it will be useful, +** but WITHOUT ANY WARRANTY; without even the implied warranty of +** MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU +** Library General Public License for more details. +** +** You should have received a copy of the GNU Library General Public +** License along with this library; if not, write to the +** Free Software Foundation, Inc., 59 Temple Place - Suite 330, +** Boston, MA 02111-1307, USA. +********************************************************************/ +#include "gps.h" +#include +#include +#include + + +/* @func GPS_Make_Packet *********************************************** +** +** Forms a complete packet to send +** +** @param [w] packet [GPS_PPacket *] packet string +** @param [r] type [UC] packet type +** @param [r] data [UC *] data string +** @param [r] n [int16] number of bytes in data string +** +** @return [void] +************************************************************************/ + +void GPS_Make_Packet(GPS_PPacket *packet, UC type, UC *data, int16 n) +{ + UC *p; + UC *q; + + int32 i; + UC chk=0; + + p = data; + q = (*packet)->data; + + (*packet)->dle = DLE; + (*packet)->edle = DLE; + (*packet)->etx = ETX; + (*packet)->n = n; + (*packet)->type = type; + (*packet)->bytes = 0; + + chk -= type; + + if(n == DLE) + { + ++(*packet)->bytes; + *q++ = DLE; + } + + + chk -= (UC) n; + + for(i=0;ibytes; + *q++ = DLE; + } + chk -= *p; + *q++ = *p++; + ++(*packet)->bytes; + } + + if(chk == DLE) + { + *q++ = DLE; + ++(*packet)->bytes; + } + + (*packet)->chk = chk; + + return; +} + + + + +/* @func GPS_Write_Packet *********************************************** +** +** Forms a complete packet to send +** +** @param [w] fd [int32] file descriptor +** @param [r] packet [GPS_PPacket] packet +** +** @return [int32] number of bytes in the packet +************************************************************************/ + +int32 GPS_Write_Packet(int32 fd, GPS_PPacket packet) +{ + size_t ret; + + if((ret=GPS_Serial_Write(fd,(const void *)&packet->dle,(size_t)3)) == -1) + { + perror("write"); + GPS_Error("SEND: Write to GPS failed"); + return 0; + } + if(ret!=3) + { + GPS_Error("SEND: Incomplete write to GPS"); + return 0; + } + + if((ret=GPS_Serial_Write(fd,(const void *)packet->data,(size_t)packet->bytes)) == -1) + { + perror("write"); + GPS_Error("SEND: Write to GPS failed"); + return 0; + } + if(ret!=packet->bytes) + { + GPS_Error("SEND: Incomplete write to GPS"); + return 0; + } + + + if((ret=GPS_Serial_Write(fd,(const void *)&packet->chk,(size_t)3)) == -1) + { + perror("write"); + GPS_Error("SEND: Write to GPS failed"); + return 0; + } + if(ret!=3) + { + GPS_Error("SEND: Incomplete write to GPS"); + return 0; + } + + + return 1; +} + + +/* @func GPS_Send_Ack *********************************************** +** +** Send an acknowledge packet +** +** @param [w] fd [int32] file descriptor +** @param [r] tra [GPS_PPacket *] packet to transmit +** @param [r] rec [GPS_PPacket *] last packet received +** +** @return [int32] success +************************************************************************/ + +int32 GPS_Send_Ack(int32 fd, GPS_PPacket *tra, GPS_PPacket *rec) +{ + UC data[2]; + + GPS_Util_Put_Short(data,(US)(*rec)->type); + GPS_Make_Packet(tra,LINK_ID[0].Pid_Ack_Byte,data,2); + if(!GPS_Write_Packet(fd,*tra)) + { + GPS_Error("Error acknowledging packet"); + gps_errno = SERIAL_ERROR; + return 0; + } + + return 1; +} diff --git a/gpsbabel/jeeps/gpssend.h b/gpsbabel/jeeps/gpssend.h new file mode 100644 index 000000000..f048fdec8 --- /dev/null +++ b/gpsbabel/jeeps/gpssend.h @@ -0,0 +1,26 @@ +#ifdef __cplusplus +extern "C" +{ +#endif + +#ifndef gpssend_h +#define gpssend_h + + +#include "gps.h" + +#define GPS_ARB_LEN 1024 + +UC gps_sendbuf[GPS_ARB_LEN]; + +void GPS_Make_Packet(GPS_PPacket *packet, UC type, UC *data, int16 n); +int32 GPS_Write_Packet(int32 fd, GPS_PPacket packet); +int32 GPS_Send_Ack(int32 fd, GPS_PPacket *tra, GPS_PPacket *rec); + + + +#endif + +#ifdef __cplusplus +} +#endif diff --git a/gpsbabel/jeeps/gpsserial.c b/gpsbabel/jeeps/gpsserial.c new file mode 100644 index 000000000..b3f8e37bb --- /dev/null +++ b/gpsbabel/jeeps/gpsserial.c @@ -0,0 +1,542 @@ +/******************************************************************** +** @source JEEPS serial port low level functions +** +** @author Copyright (C) 1999,2000 Alan Bleasby +** @version 1.0 +** @modified December 28th 1999 Alan Bleasby. First version +** @modified June 29th 2000 Alan Bleasby. NMEA additions +** @@ +** +** This library is free software; you can redistribute it and/or +** modify it under the terms of the GNU Library General Public +** License as published by the Free Software Foundation; either +** version 2 of the License, or (at your option) any later version. +** +** This library is distributed in the hope that it will be useful, +** but WITHOUT ANY WARRANTY; without even the implied warranty of +** MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU +** Library General Public License for more details. +** +** You should have received a copy of the GNU Library General Public +** License along with this library; if not, write to the +** Free Software Foundation, Inc., 59 Temple Place - Suite 330, +** Boston, MA 02111-1307, USA. +********************************************************************/ +#include "gps.h" +#include +#include +#include +#include +#include +#include +#include +#include + + +#if __WIN32__ + +#include +/* + * Rather than teaching the rest of this code about Windows serial APIs + * we'll weenie out, violate good layering, and just keep our handle + * internal. This means we ignore that 'fd' number that gets passed in. + */ + +static HANDLE comport; + +int32 GPS_Serial_On(const char *port, int32 *fd) +{ + DCB tio; + COMMTIMEOUTS timeout; + + comport = CreateFile(port, GENERIC_READ|GENERIC_WRITE, 0, NULL, + OPEN_EXISTING, 0, NULL); + if (comport == INVALID_HANDLE_VALUE) { + gps_errno = SERIAL_ERROR; + return 0; + } + + tio.DCBlength = sizeof(DCB); + GetCommState (comport, &tio); + tio.BaudRate = CBR_9600; + tio.fBinary = TRUE; + tio.fParity = TRUE; + tio.fOutxCtsFlow = FALSE; + tio.fOutxDsrFlow = FALSE; + tio.fDtrControl = DTR_CONTROL_ENABLE; + tio.fDsrSensitivity = FALSE; + tio.fTXContinueOnXoff = TRUE; + tio.fOutX = FALSE; + tio.fInX = FALSE; + tio.fErrorChar = FALSE; + tio.fNull = FALSE; + tio.fRtsControl = RTS_CONTROL_ENABLE; + tio.fAbortOnError = FALSE; + tio.ByteSize = 8; + tio.Parity = NOPARITY; + tio.StopBits = ONESTOPBIT; + + if (!SetCommState (comport, &tio)) { + CloseHandle(comport); + gps_errno = SERIAL_ERROR; + return 0; + } + + GetCommTimeouts (comport, &timeout); + timeout.ReadIntervalTimeout = 10; + timeout.WriteTotalTimeoutMultiplier = 10; + timeout.WriteTotalTimeoutConstant = 1000; + if (!SetCommTimeouts (comport, &timeout)) { + CloseHandle (comport); + gps_errno = SERIAL_ERROR; + return 0; + } + *fd = 1; + return 1; +} + +int32 GPS_Serial_Off(const char *port, int32 fd) +{ + CloseHandle(comport); + return 1; +} + +int32 GPS_Serial_Chars_Ready(int32 fd) +{ + return 1; +} + +int32 GPS_Serial_Wait(int32 fd) +{ + return 1; +} + +int32 GPS_Serial_Flush(int32 fd) +{ + return 1; +} + +int32 GPS_Serial_Write(int ignored, void *obuf, int size) +{ + DWORD len; + WriteFile (comport, obuf, size, &len, NULL); + if (len != size) { + fatal ("Write error. Wrote %d of %d bytes.", len, size); + } + return len; +} + +int GPS_Serial_Read(int ignored, void *ibuf, int size) +{ + DWORD cnt; + + ReadFile(comport, ibuf, size, &cnt, NULL); + return cnt; +} + +#else + +#include +#include + +static struct termios gps_ttysave; + +/* @func GPS_Serial_Restoretty *********************************************** +** +** Save tty information for the serial post to be used +** +** @param [r] port [const char *] port e.g. ttyS1 +** +** @return [int32] false upon error +************************************************************************/ + +int32 GPS_Serial_Savetty(const char *port) +{ + int32 fd; + + if((fd = open(port, O_RDWR|O_NDELAY))==-1) + { + perror("open"); + gps_errno = SERIAL_ERROR; + GPS_Error("SERIAL: Cannot open serial port"); + return 0; + } + + if(tcgetattr(fd,&gps_ttysave)==-1) + { + perror("tcgetattr"); + gps_errno = HARDWARE_ERROR; + GPS_Error("SERIAL: tcgetattr error"); + return 0; + } + + + if(!GPS_Serial_Close(fd,port)) + { + gps_errno = SERIAL_ERROR; + GPS_Error("GPS_Serial_Savetty: Close error"); + return 0; + } + + return 1; +} + + +/* @func GPS_Serial_Restoretty *********************************************** +** +** Restore serial post to condition before AJBGPS called +** +** @param [r] port [const char *] port e.g. ttyS1 +** +** @return [int32] false upon error +************************************************************************/ + +int32 GPS_Serial_Restoretty(const char *port) +{ + int32 fd; + + if((fd = open(port, O_RDWR|O_NDELAY))==-1) + { + perror("open"); + gps_errno = HARDWARE_ERROR; + GPS_Error("SERIAL: Cannot open serial port"); + return 0; + } + + if(tcsetattr(fd, TCSAFLUSH, &gps_ttysave)==-1) + { + perror("ioctl"); + gps_errno = HARDWARE_ERROR; + GPS_Error("SERIAL: tcsetattr error"); + return 0; + } + + return 1; +} + + + +/* @func GPS_Serial_Open *********************************************** +** +** Open a serial port 8bits 1 stop bit 9600 baud +** +** @param [w] fd [int32 *] file descriptor +** @param [r] port [const char *] port e.g. ttyS1 +** +** @return [int32] false upon error +************************************************************************/ + +int32 GPS_Serial_Open(int32 *fd, const char *port) +{ + struct termios tty; + + + if((*fd = open(port, O_RDWR | O_NDELAY | O_NOCTTY))==-1) + { + perror("open"); + GPS_Error("SERIAL: Cannot open serial port"); + gps_errno = SERIAL_ERROR; + return 0; + } + + + if(tcgetattr(*fd,&tty)==-1) + { + perror("tcgetattr"); + GPS_Error("SERIAL: tcgetattr error"); + gps_errno = SERIAL_ERROR; + return 0; + } + + tty.c_cflag |= (CREAD | CS8 | CSIZE | CLOCAL); + cfsetospeed(&tty,B9600); + cfsetispeed(&tty,B9600); + + tty.c_lflag &= 0x0; + tty.c_iflag &= 0x0; + tty.c_oflag &= 0x0; + + + if(tcsetattr(*fd,TCSANOW,&tty)==-1) + { + perror("tcsetattr"); + GPS_Error("SERIAL: tcsetattr error"); + return 0; + } + + return 1; +} + +int32 GPS_Serial_Read(int32 handle, void *ibuf, int size) +{ + return read(handle, ibuf, size); +} + +int32 GPS_Serial_Write(int32 handle, void *obuf, int size) +{ + return write(handle, obuf, size); +} + + +/* @func GPS_Serial_Flush *********************************************** +** +** Flush the serial lines +** +** @param [w] fd [int32] file descriptor +** +** @return [int32] false upon error +************************************************************************/ +int32 GPS_Serial_Flush(int32 fd) +{ + + if(tcflush(fd,TCIOFLUSH)) + { + perror("tcflush"); + GPS_Error("SERIAL: tcflush error"); + gps_errno = SERIAL_ERROR; + return 0; + } + + return 1; +} + + + +/* @func GPS_Serial_Close *********************************************** +** +** Close serial port +** +** @param [r] fd [int32 ] file descriptor +** @param [r] port [const char *] port e.g. ttyS1 +** +** @return [int32] false upon error +************************************************************************/ + +int32 GPS_Serial_Close(int32 fd, const char *port) +{ + if(close(fd)==-1) + { + perror("close"); + GPS_Error("SERIAL: Error closing serial port"); + gps_errno = SERIAL_ERROR; + return 0; + } + + return 1; +} + + +/* @func GPS_Serial_Chars_Ready ***************************************** +** +** Query port to see if characters are waiting to be read +** +** @param [r] fd [int32 ] file descriptor +** +** @return [int32] true if chars waiting +************************************************************************/ + +int32 GPS_Serial_Chars_Ready(int32 fd) +{ + fd_set rec; + struct timeval t; + + FD_ZERO(&rec); + FD_SET(fd,&rec); + + t.tv_sec = 0; + t.tv_usec = 0; + + (void) select(fd+1,&rec,NULL,NULL,&t); + if(FD_ISSET(fd,&rec)) + return 1; + + return 0; +} + + + +/* @func GPS_Serial_Wait *********************************************** +** +** Wait 80 milliseconds before testing for input. The GPS delay +** appears to be around 40-50 milliseconds. Doubling the value is to +** allow some leeway. +** +** @param [r] fd [int32 ] file descriptor +** +** @return [int32] true if serial chars waiting +************************************************************************/ + +int32 GPS_Serial_Wait(int32 fd) +{ + fd_set rec; + struct timeval t; + + FD_ZERO(&rec); + FD_SET(fd,&rec); + + t.tv_sec = 0; + t.tv_usec = usecDELAY; + + (void) select(fd+1,&rec,NULL,NULL,&t); + if(FD_ISSET(fd,&rec)) + return 1; + + return 0; +} + + + +/* @func GPS_Serial_On ***************************************** +** +** Set up port +** +** @param [r] port [const char *] port +** @param [w] fd [int32 *] file descriptor +** +** @return [int32] success +************************************************************************/ + +int32 GPS_Serial_On(const char *port, int32 *fd) +{ + + if(!GPS_Serial_Savetty(port)) + { + GPS_Error("Cannot access serial port"); + gps_errno = SERIAL_ERROR; + return 0; + } + + if(!GPS_Serial_Open(fd,port)) + { + GPS_Error("Cannot open serial port"); + gps_errno = SERIAL_ERROR; + return 0; + } + + return 1; +} + + + +/* @func GPS_Serial_Off *********************************************** +** +** Done with port +** +** @param [r] port [const char *] port +** @param [r] fd [int32 ] file descriptor +** +** @return [int32] success +************************************************************************/ + +int32 GPS_Serial_Off(const char *port, int32 fd) +{ + if(!GPS_Serial_Close(fd,port)) + { + GPS_Error("Error Closing port"); + gps_errno = HARDWARE_ERROR; + return 0; + } + + if(!GPS_Serial_Restoretty(port)) + { + GPS_Error("Error restoring port"); + gps_errno = HARDWARE_ERROR; + return 0; + } + + return 1; +} + + + + + + + +/* @func GPS_Serial_Open_NMEA ****************************************** +** +** Open a serial port 8bits 1 stop bit 4800 baud +** +** @param [w] fd [int32 *] file descriptor +** @param [r] port [const char *] port e.g. ttyS1 +** +** @return [int32] false upon error +************************************************************************/ + +int32 GPS_Serial_Open_NMEA(int32 *fd, const char *port) +{ + struct termios tty; + + + if((*fd = open(port, O_RDWR | O_NDELAY | O_NOCTTY))==-1) + { + perror("open"); + GPS_Error("SERIAL: Cannot open serial port"); + gps_errno = SERIAL_ERROR; + return 0; + } + + + if(tcgetattr(*fd,&tty)==-1) + { + perror("tcgetattr"); + GPS_Error("SERIAL: tcgetattr error"); + gps_errno = SERIAL_ERROR; + return 0; + } + + + tty.c_cflag |= (CREAD | CS8 | CSIZE | CLOCAL); + cfsetospeed(&tty,B4800); + cfsetispeed(&tty,B4800); + + tty.c_lflag &= 0x0; + tty.c_iflag &= 0x0; + tty.c_oflag &= 0x0; + + + if(tcsetattr(*fd,TCSANOW,&tty)==-1) + { + perror("tcsetattr"); + GPS_Error("SERIAL: tcsetattr error"); + return 0; + } + + return 1; +} + + + + + + + +/* @func GPS_Serial_On_NMEA ******************************************** +** +** Set up port for NMEA +** +** @param [r] port [const char *] port +** @param [w] fd [int32 *] file descriptor +** +** @return [int32] success +************************************************************************/ +int32 GPS_Serial_On_NMEA(const char *port, int32 *fd) +{ + + if(!GPS_Serial_Savetty(port)) + { + GPS_Error("Cannot access serial port"); + gps_errno = SERIAL_ERROR; + return 0; + } + + if(!GPS_Serial_Open_NMEA(fd,port)) + { + GPS_Error("Cannot open serial port"); + gps_errno = SERIAL_ERROR; + return 0; + } + + return 1; +} +#endif /* __WIN32__ */ diff --git a/gpsbabel/jeeps/gpsserial.h b/gpsbabel/jeeps/gpsserial.h new file mode 100644 index 000000000..b1f370185 --- /dev/null +++ b/gpsbabel/jeeps/gpsserial.h @@ -0,0 +1,31 @@ +#ifdef __cplusplus +extern "C" +{ +#endif + +#ifndef gpsserial_h +#define gpsserial_h + + +#include "gps.h" + +#define usecDELAY 180000 /* Microseconds before GPS sends A001 */ + +int32 GPS_Serial_Chars_Ready(int32 fd); +int32 GPS_Serial_Close(int32 fd, const char *port); +int32 GPS_Serial_Open(int32 *fd, const char *port); +int32 GPS_Serial_Open_NMEA(int32 *fd, const char *port); +int32 GPS_Serial_Restoretty(const char *port); +int32 GPS_Serial_Savetty(const char *port); +int32 GPS_Serial_On(const char *port, int32 *fd); +int32 GPS_Serial_Off(const char *port, int32 fd); +int32 GPS_Serial_Wait(int32 fd); +int32 GPS_Serial_Flush(int32 fd); +int32 GPS_Serial_On_NMEA(const char *port, int32 *fd); + + +#endif + +#ifdef __cplusplus +} +#endif diff --git a/gpsbabel/jeeps/gpsutil.c b/gpsbabel/jeeps/gpsutil.c new file mode 100644 index 000000000..218e84ed7 --- /dev/null +++ b/gpsbabel/jeeps/gpsutil.c @@ -0,0 +1,681 @@ +/******************************************************************** +** @source JEEPS utility functions +** +** @author Copyright (C) 1999 Alan Bleasby +** @version 1.0 +** @modified Dec 28 1999 Alan Bleasby. First version +** @@ +** +** This library is free software; you can redistribute it and/or +** modify it under the terms of the GNU Library General Public +** License as published by the Free Software Foundation; either +** version 2 of the License, or (at your option) any later version. +** +** This library is distributed in the hope that it will be useful, +** but WITHOUT ANY WARRANTY; without even the implied warranty of +** MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU +** Library General Public License for more details. +** +** You should have received a copy of the GNU Library General Public +** License along with this library; if not, write to the +** Free Software Foundation, Inc., 59 Temple Place - Suite 330, +** Boston, MA 02111-1307, USA. +********************************************************************/ +#include "gps.h" +// #include +#include + +static int32 gps_endian_called=0; +static int32 GPS_Little=0; + +int32 gps_warning = 0; +int32 gps_error = 0; +int32 gps_user = 0; +int32 gps_show_bytes = 0; +int32 gps_errno = 0; + + +/* @func GPS_Util_Little *********************************************** +** +** Determine endian nature of host +** +** @return [int32] true if little-endian +************************************************************************/ + +int32 GPS_Util_Little(void) +{ + static union lb + { + char chars[sizeof(int32)]; + int32 i; + } + data; + + if(!gps_endian_called) + { + gps_endian_called = 1; + data.i = 0; + *data.chars = '\1'; + if(data.i == 1) + GPS_Little = 1; + else + GPS_Little = 0; + } + + return GPS_Little; +} + + +/* @func GPS_Util_Get_Short ******************************************** +** +** Get a short from a string +** +** @return [US] value +************************************************************************/ + +US GPS_Util_Get_Short(const UC *s) +{ + static US ret; + UC *p; + + p = (UC *)&ret; + + if(!GPS_Little) + { + *p++ = *(s+1); + *p = *s; + } + else + { + *p++ = *s; + *p = *(s+1); + } + + return ret; +} + + + +/* @func GPS_Util_Put_Short ******************************************** +** +** Put a short to a string +** +** @param [w] s [UC *] string to write to +** @param [r] v [const US] short to write +** +** @return [void] +************************************************************************/ + +void GPS_Util_Put_Short(UC *s, const US v) +{ + UC *p; + + p = (UC *)&v; + + if(!GPS_Little) + { + *s++ = *(p+1); + *s = *p; + } + else + { + *s++ = *p; + *s = *(p+1); + } + + return; +} + + + +/* @func GPS_Util_Get_Double ******************************************** +** +** Get a double from a string +** +** @return [double] value +************************************************************************/ + +double GPS_Util_Get_Double(const UC *s) +{ + double ret; + UC *p; + int32 i; + + p = (UC *)&ret; + + + if(!GPS_Little) + for(i=sizeof(double)-1;i>-1;--i) + *p++ = s[i]; + else + for(i=0;i-1;--i) + s[i] = *p++; + else + for(i=0;i-1;--i) + *p++ = s[i]; + else + for(i=0;i-1;--i) + s[i] = *p++; + else + for(i=0;i-1;--i) + *p++ = s[i]; + else + for(i=0;i-1;--i) + s[i] = *p++; + else + for(i=0;i-1;--i) + *p++ = s[i]; + else + for(i=0;i-1;--i) + s[i] = *p++; + else + for(i=0;iraw state=false->normal +** @return [void] +** @@ +****************************************************************************/ + +void GPS_Util_Canon(int32 state) +{ + static struct termios tty; + static struct termios sv; + + + if(state) + { + tcgetattr(1,&sv); + tcgetattr(1, &tty); + tty.c_cc[VMIN]='\1'; + tty.c_cc[VTIME]='\0'; + tcsetattr(1,TCSANOW,&tty); + tty.c_lflag &= ~(ICANON | ECHO); + tcsetattr(1, TCSANOW, &tty); + } + else + tcsetattr(1, TCSANOW, &sv); + + return; +} +#endif + +#if 0 +/* @func GPS_Util_Block **************************************************** +** +** Sets or unsets blocking +** @modified 13-01-2000 to return an int +** +** @param [r] fd [int32] file descriptor +** @param [r] state [int32] state=true->block state=false->non-block +** +** @return [int32] success +** @@ +****************************************************************************/ + +int32 GPS_Util_Block(int32 fd, int32 state) +{ + static int32 notcalled=1; + static int32 block; + static int32 noblock; + int32 f; + + gps_errno = HARDWARE_ERROR; + + if(notcalled) + { + notcalled = 0; + if((f=fcntl(fd,F_GETFL,0))==-1) + { + GPS_Error("Util_Block: FCNTL error"); + return 0; + } + block = f & ~O_NDELAY; + noblock = f | O_NDELAY; + } + + if(state) + { + if(fcntl(fd,F_SETFL,block)==-1) + { + GPS_Error("Util_Block: Error blocking"); + return 0; + } + } + else + { + if(fcntl(fd,F_SETFL,noblock)==-1) + { + GPS_Error("Util_Block: Error unblocking"); + return 0; + } + } + + return 1; +} +#endif + + +/* @func GPS_Warning ******************************************************** +** +** Prints warning if gps_warning is true +** +** @param [r] s [char *] warning +** +** @return [void] +** @@ +****************************************************************************/ + +void GPS_Warning(char *s) +{ + if(!gps_warning) + return; + + fprintf(stderr,"[WARNING] %s\n",s); + fflush(stderr); + + return; +} + + +/* @func GPS_Fatal ******************************************************** +** +** Always prints error and exits program +** Bad thing for a library so the library doesn't call it. +** +** @param [r] s [char *] fatal error +** +** @return [void] +** @@ +****************************************************************************/ + +void GPS_Fatal(char *s) +{ + + fprintf(stderr,"[FATAL] %s\n",s); + exit(0); + return; +} + + + +/* @func GPS_Error ********************************************************** +** +** Prints Error if gps_error is true +** +** @param [r] s [char *] error +** +** @return [void] +** @@ +****************************************************************************/ + +void GPS_Error(char *s) +{ + if(!gps_error) + return; + + fprintf(stderr,"[ERROR] %s\n",s); + fflush(stderr); + + return; +} + + +/* @func GPS_Enable_Error *************************************************** +** +** Enable error message printing +** +** @return [void] +** @@ +****************************************************************************/ + +void GPS_Enable_Error(void) +{ + gps_error = 1; + return; +} + + + +/* @func GPS_Enable_Warning *************************************************** +** +** Enable warning message printing +** +** @return [void] +** @@ +****************************************************************************/ + +void GPS_Enable_Warning(void) +{ + gps_warning = 1; + return; +} + + + +/* @func GPS_Disable_Error *************************************************** +** +** Disable error message printing +** +** @return [void] +** @@ +****************************************************************************/ + +void GPS_Disable_Error(void) +{ + gps_error = 0; + return; +} + + + +/* @func GPS_Disable_Warning *********************************************** +** +** Disable warning message printing +** +** @return [void] +** @@ +****************************************************************************/ + +void GPS_Disable_Warning(void) +{ + gps_warning = 0; + return; +} + + + +/* @func GPS_User ******************************************************** +** +** Prints a message if gps_user is true +** +** @param [r] s [char *] message +** +** @return [void] +** @@ +****************************************************************************/ + +void GPS_User(char *s) +{ + if(!gps_user) + return; + + fprintf(stdout,"%s\n",s); + fflush(stdout); + + return; +} + +/* @func GPS_Disable_User *********************************************** +** +** Disable message printing +** +** @return [void] +** @@ +****************************************************************************/ + +void GPS_Disable_User(void) +{ + gps_user = 0; + return; +} + + +/* @func GPS_Enable_User *********************************************** +** +** Disable warning message printing +** +** @return [void] +** @@ +****************************************************************************/ + +void GPS_Enable_User(void) +{ + gps_user = 1; + return; +} + + +/* @func GPS_Diagnose ******************************************************** +** +** Prints bytes read from gps if gps_show_bytes is set +** +** @param [r] cs [int32] byte read +** +** @return [void] +** @@ +****************************************************************************/ + +void GPS_Diagnose(int32 c) +{ + if(!gps_show_bytes) + return; + + fprintf(stdout,"%d\n",(int)c); + fflush(stdout); + + return; +} + + + +/* @func GPS_Enable_Diagnose *********************************************** +** +** Enable diagnosis mode +** +** @return [void] +** @@ +****************************************************************************/ + +void GPS_Enable_Diagnose(void) +{ + gps_show_bytes = 1; + return; +} + + + +/* @func GPS_Disble_Diagnose *********************************************** +** +** Disable diagnosis mode +** +** @return [void] +** @@ +****************************************************************************/ + +void GPS_Disable_Diagnose(void) +{ + gps_show_bytes = 0; + return; +} diff --git a/gpsbabel/jeeps/gpsutil.h b/gpsbabel/jeeps/gpsutil.h new file mode 100644 index 000000000..245824208 --- /dev/null +++ b/gpsbabel/jeeps/gpsutil.h @@ -0,0 +1,46 @@ +#ifdef __cplusplus +extern "C" +{ +#endif + +#ifndef gpsutil_h +#define gpsutil_h + + +#include "gps.h" + +int32 GPS_Util_Little(void); + +US GPS_Util_Get_Short(const UC *s); +void GPS_Util_Put_Short(UC *s, const US v); +int32 GPS_Util_Get_Int(const UC *s); +void GPS_Util_Put_Int(UC *s, const int32 v); +double GPS_Util_Get_Double(const UC *s); +void GPS_Util_Put_Double(UC *s, const double v); +float GPS_Util_Get_Float(const UC *s); +void GPS_Util_Put_Float(UC *s, const float v); +void GPS_Util_Canon(int32 state); +int32 GPS_Util_Block(int32 fd, int32 state); +void GPS_Util_Put_Uint(UC *s, const uint32 v); +uint32 GPS_Util_Get_Uint(const UC *s); + +void GPS_Warning(char *s); +void GPS_Error(char *s); +void GPS_Fatal(char *s); +void GPS_Enable_Error(void); +void GPS_Enable_Warning(void); +void GPS_Disable_Error(void); +void GPS_Disable_Warning(void); +void GPS_User(char *s); +void GPS_Disable_User(void); +void GPS_Enable_User(void); +void GPS_Diagnose(int32 c); +void GPS_Enable_Diagnose(void); +void GPS_Disable_Diagnose(void); + + +#endif + +#ifdef __cplusplus +} +#endif diff --git a/gpsbabel/jeeps/main.c b/gpsbabel/jeeps/main.c new file mode 100644 index 000000000..a8488248b --- /dev/null +++ b/gpsbabel/jeeps/main.c @@ -0,0 +1,31 @@ +#include "gps.h" +// #include "jeeps.h" + +main() +{ + int n; + GPS_PWay *way; + GPS_PWay *array; + + if (GPS_Init("/dev/ttyS0") < 0) { + fprintf(stderr, "Can't init\n"); + } + + if((n=GPS_Command_Get_Waypoint("/dev/ttyS0", &way))<0) { + fprintf(stderr, "can't get\n"); + return; + } +// fprintf(stdout," Done\n"); + + GPS_Fmt_Print_Waypoint(way, n, stdout); + + array = (GPS_PWay *) calloc(1, sizeof(GPS_PWay)); + array[0] = GPS_Way_New(); + strcpy(array[0]->ident,"lower @#$%^&* rocks"); + strcpy(array[0]->cmnt,"COMMENTCOMMENTCOMMENTCOMMENTCOMMENT"); + array[0]->wpt_class = 0; + array[0]->lat = 1.234; + array[0]->lon = 1.234; +GPS_Command_Send_Waypoint("/dev/ttyS0", array, 1); + +} diff --git a/gpsbabel/mingw/Makefile b/gpsbabel/mingw/Makefile index b8524bd19..52465f0b2 100644 --- a/gpsbabel/mingw/Makefile +++ b/gpsbabel/mingw/Makefile @@ -1,4 +1,4 @@ -CC=/home/robertl/cross-tools/bin/i386-mingw32msvc-gcc -Iinclude -I../coldsync -DCETUS +CC=/home/robertl/cross-tools/bin/i386-mingw32msvc-gcc -Iinclude VPATH=.. gpsbabel.exe: diff --git a/gpsbabel/mkshort.c b/gpsbabel/mkshort.c index ffe954a45..a048ca852 100644 --- a/gpsbabel/mkshort.c +++ b/gpsbabel/mkshort.c @@ -11,6 +11,7 @@ static int target_len = DEFAULT_TARGET_LEN; static const char *badchars = DEFAULT_BADCHARS; static int mustupper = 0; +static int whitespaceok = 1; static const char needmem[] = "mkshort: could not reallocate memory for string\n"; @@ -56,6 +57,12 @@ setshort_length(int l) } } +void +setshort_whitespace_ok(int l) +{ + whitespaceok = l; +} + /* * Externally callable function to set the string of characters * that must never appear in a string returned by mkshort. NULL @@ -95,8 +102,9 @@ mkshort(char *istring) /* * Whack leading "[Tt]he", */ - if (strncmp(ostring, "The ", 4) == 0 || - strncmp(ostring, "the ", 4) == 0) { + if (( strlen(ostring) > target_len + 4) && + (strncmp(ostring, "The ", 4) == 0 || + strncmp(ostring, "the ", 4) == 0)) { nstring = strdup(ostring + 4); if (!nstring) { fatal(needmem); @@ -121,25 +129,27 @@ mkshort(char *istring) free(ostring); ostring = nstring; - /* - * Eliminate Whitespace - */ - tstring = strdup(ostring); - if (!tstring) { - fatal(needmem); - } - l = strlen (tstring); - cp = ostring; - for (i=0;i #include /* for M_PI */ +#ifndef M_PI +# define M_PI 3.141592653589 +#endif + #define MYNAME "PSP" #define MAXPSPSTRINGSIZE 256 diff --git a/gpsbabel/vecs.c b/gpsbabel/vecs.c index 28f4f050c..6dceb8837 100644 --- a/gpsbabel/vecs.c +++ b/gpsbabel/vecs.c @@ -41,6 +41,7 @@ extern ff_vecs_t csv_vecs; extern ff_vecs_t cetus_vecs; extern ff_vecs_t gpspilot_vecs; extern ff_vecs_t psp_vecs; +extern ff_vecs_t garmin_vecs; extern ff_vecs_t mxf_vecs; extern ff_vecs_t holux_vecs; @@ -111,6 +112,11 @@ vecs_t vec_list[] = { "gpspilot", "GPSPilot Tracker for Palm/OS" }, + { + &garmin_vecs, + "garmin", + "Garmin serial protocol" + }, { &mxf_vecs, "mxf", -- 2.30.2